问:
Controlling the pose of a manipulator using inverse kinematics
I want to control a robotic manipulator in Cartesian space, using inverse kinematics. I know that I can do this using $\omega=J^{-1}v$, where $\omega$ is a vector of joint velocities, $J$ is the Jacobian, and $v$ is a vector of {position, orientation} velocities in Cartesian space. Here, the Jacobian expresses how the Cartesian {positions, orientations} change with respect to the joint angles.

My question is as follows. What I have, is two poses of the manipulator in Cartesian space, pose $A$ and pose $B$. And I want to move the manipulator from $A$ to $B$. How do I create the vector $v$ needed for the above inverse kinematics equation?

I am confused, because currently $A$ and $B$ are transformation matrices, expressing the pose of the manipulator in the fixed world coordinates. So, somehow I need to find the difference between the two poses, and then I need to convert this "difference" into the velocity vector $v$. Can anybody help me out please?

答:
As you already have the Jacobian vector, I assume you also solved the inverse kinematics problem. I will refer to the IK problem as $f^{-1}$

In order to get the joint space equivalent of the Cartesian pose A (position and orientation, denoted as $X_A$) you can use the inverse kinematics:
$$Q_A = f^{-1}(X_A) $$
similarly
$$Q_B = f^{-1}(X_B) $$

(Please note that in most cases the IK has more then one solutions, I am just considering one, pre-selected solution here.)

Now if you give the motors the reference position $Q_A$ or $Q_B$ "eventually" you will end up in pose $A$ and in pose $B$. In this case there are no guaranties that the motors will have the same travel time and there are no guarantees about the "shape" of the path the robot will take.

You can define a straight line between $A$ and $B$ and interpolate between these points on this straight line. In this case there are solutions for an equivalent interpolation technique for the orientation also. For each of the Cartesian interpolated points you can calculate the joint space equivalent and use them as reference positions for the robot. In this case you will have a straight line path in joint space, but the path will very likely have an oscillating velocity profile as the distance to each point will eventually (in the closed loop position controller) will define the implicit velocity profile.

This can be done using IK only. If you prefer to use the $J^{-1}$ you can subtract the current pose of the robot from your target pose.

$$ D = X_B - X_{Current}$$
If we consider D a velocity vector, we can interpret it as the velocity needed to reach point B in unit time. If we are talking about millimetre and seconds, then it is mm/s.
$$\omega = J^{-1} \times D $$
 If you give this as a constant reference velocity to the robot it will travel in the direction of B and then surpass B and continue. If you cyclically re-evaluate $D$ using the current position it will slow down the robot until eventually almost reaching point $B$. You can add a multiplication factor to this equation and if it is cyclically evaluated with the updated robot positions you will have the effect of a proportional velocity controller. But the velocity profile obtained this way will still be an implicit profile, which slows down towards the end of the path (only) linearly and tries to start with a high velocity.

In order to have a more controlled velocity profile and a straight line you can design an explicit velocity profile. This is called trajectory planning and, most commonly uses an S shaped curve. More about this here.

If you define a velocity profile in Cartesian space (or Task space if we consider pose not just position) at each point in time you can evaluate the velocity profile function at one given point. This is your $v$ vector. You will need the Cartesian position or the joint space position, depending on how you derive your $J^{-1}$ it is usually $(J(Q))^{-1}$ or $J^{-1}(X)$. (One could argue that $(J(Q))^{-1}$ is more helpful as it can use the current joint space coordinates to convert the velocity vector.). To summarize: you define a path in Task space, you plan a velocity profile and you convert this velocity profile, based on the current time step and current robot pose to obtain a new velocity vector in joint space which is given to the velocity controller as a reference value.


问:
OpenCV4.2 and Ubuntu16.04 with python2.7?
Summary:

I'm using new vision of opencv, opencv 4.2 with ubuntu 16.04 and python 2.7

After built cv_bridge, the output is:

```<code>import rospkg
ImportError: No module named rospkg
</code>```

Here's the more detailed version: here

I tried to build cv_bridge in order to run OpenCV 4.2, Ubuntu 16.04 (ROS Kinetic) and python 2.7.

I will explain what I do for that.

I built it before with OpenCV 3.3.1 in order to use GPU and it was very fine and it is still working now. To best my knowledge, there is ros-kinetic-opencv3, it is working only on CPU. I removed it using sudo apt remove ros-kinetic-opencv3

After removing, I just downloaded your here and put in my ~/catkin_ws/src/, then making ~/catkin_ws/catkin_make on my PC without any modifications. I think that I was lucky.

Now, I'd like use some advantage of OpenCV 4.2 that is not with older versions like OpenCV 3.3.1. Meanwhile, I can't update robot's system, it is Ubuntu 16.04.

So, I tried to build cv_bridge, all modifications as following:

I copied and pasted module_opencv3.cpp, then change its name to module_opencv4.cpp so that became three models 4, 3 and 2.cpp instead of two 3 and 2.cpp.

Next , Sreevan mentioned here some changes, I did like Sreevan:

In cv_bridge/src/module_opencv3.cpp change signature of two functions

3.1) UMatData* allocate(int dims0, const int* sizes, int type, void* data, size_t* step, int flags, UMatUsageFlags usageFlags) const
to
UMatData* allocate(int dims0, const int* sizes, int type, void* data, size_t* step, AccessFlag flags, UMatUsageFlags usageFlags) const

3.2) bool allocate(UMatData* u, int accessFlags, UMatUsageFlags usageFlags) const
to
bool allocate(UMatData* u, AccessFlag accessFlags, UMatUsageFlags usageFlags) const

Next, I went to this file ~/catkin_ws/src/vision_opencv/cv_bridge$ CMakeLists.txt and change it as:
from:

```<code>find_package(OpenCV 3 REQUIRED
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  PATHS /usr/local
  NO_DEFAULT_PATH
  CONFIG
)
</code>```

to:

```<code>if ( NOT (OpenCV_VERSION_MAJOR VERSION_LESS 4) )
find_package(OpenCV REQUIRED
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  #PATHS /usr/local/include
  PATHS /usr/local
  NO_DEFAULT_PATH
  CONFIG
)
else()
find_package(OpenCV 3 REQUIRED
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  PATHS /usr/local
  NO_DEFAULT_PATH
  CONFIG
)
endif()
</code>```

Next, I went to this file ~/catkin_ws/src/vision_opencv/cv_bridge/src$ CMakeLists.txt and change it as:
from:

```<code>if (OpenCV_VERSION_MAJOR VERSION_EQUAL 3)
  add_library(<span class="math-container">${PROJECT_NAME}_boost module.cpp module_opencv3.cpp)
else()
  add_library($</span>{PROJECT_NAME}_boost module.cpp module_opencv2.cpp)
endif()
</code>```

to

```<code>if (OpenCV_VERSION_MAJOR VERSION_EQUAL 4)
  add_library(<span class="math-container">${PROJECT_NAME}_boost module.cpp module_opencv4.cpp)
elseif(OpenCV_VERSION_MAJOR VERSION_EQUAL 3)
  add_library($</span>{PROJECT_NAME}_boost module.cpp module_opencv3.cpp)
else()
  add_library(${PROJECT_NAME}_boost module.cpp module_opencv2.cpp)
endif()
</code>```

Next, I think this step is not important, I went to this file ~/catkin_ws/src/vision_opencv/cv_bridge$ package.xml and change any opencv3 to opencv4

Finally, ~/catkin_ws$ catkin_make

everything was ok but when I run the code:

```<code>import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

print(cv2.__version__)

class CVControl:
  def __init__(self):
    self.bridge = CvBridge()
    self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.img_callback)
  def img_callback(self, data):
    try:
      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print e

    frame = cv_image
    cv2.imshow("Image window", frame)
    cv2.waitKey(3)
def main():
  ctrl = CVControl()
  rospy.init_node('image_converter')
  try:
    rospy.spin()

  except KeyboardInterrupt:
    print "Shutting down"
    cv2.destroyAllWindows()

if __name__ == '__main__':
  main()
</code>```

It runs by OpenCV 3.3.1

If I need to run it by OpenCV 4.2
I added those lines in first the code:

```<code>import sys
ros_path2 = '/usr/local/lib/python2.7/site-packages'
ros_path3 = '/usr/lib/python2.7/dist-packages'
if  ros_path2 and ros_path3 in sys.path:
  sys.path.remove(ros_path2)
  sys.path.remove(ros_path3)
</code>```

The output is:

```<code>import rospkg
ImportError: No module named rospkg
</code>```

What is wrong?

答:
Please try this. It would install rospkg to your computer. (I have faced this issue too in the past, and this would most likely solve it)

sudo apt-get install python-rospkg

ContentLicense: CC BY-SA 4.0


答:
If I understand correctly, your end effector is at pose A, you want to move it to pose B, and you have Jacobian pseudo-inverse control set up so that you can specify a pose velocity $v$ and get a corresponding joint velocity $\omega$ that will produce that pose velocity.

There are several ways to produce the $v$ vector. All of them start by defining a time period $T$ for the motion. Your options then include

Take a simple difference of the poses:

a. Take the difference between the position components of the start and end poses, divide by $T$, and use this as the position component of your velocity,
$$ v_{\text{pos}} = \frac{B_{\text{pos}}-A_{\text{pos}}}{T}.$$
b. Take a difference between the orientation parameters of $A$ and $B$, and divide by the time period to get the rotational component of your velocity,
$$ v_{\text{rot}} = \frac{B_{\text{rot}}-A_{\text{rot}}}{T}.$$
The orientation path you get from following this velocity will depend on your orientation parameterization. Some parameterizations (e.g., quaternions) will give you smooth interpolations, but others (e.g., Euler angles) may end up looking weird and awkward.

You can construct your Jacobian so that it gives the body velocity of the end effector (its velocity with respect to the world, in coordinates instantaneously aligned with the end-effector frame). If you have a world-frame Jacobian, you can get the body frame Jacobian by using the inverse of the end-effector's rotation matrix to rotate from world to local coordinates,
$$
J_{b} = \begin{bmatrix} R_{E}^{-1} & \\ & R_{E}^{-1} \end{bmatrix} J_{w}.
$$
Once you have done this, you can find the local transformation from $A$ to $B$ (the pose that B would have if the origin were at $A$) by multiplying $B$ by the inverse of $A$,
$$
\Delta^{b} = A^{-1}B.
$$
Taking the matrix log of $\Delta^{b}$,
$$
\delta^{b} = \log_{m}(\Delta^{b})
$$
gives you a matrix whose components are
$$
\delta^{b} = \begin{bmatrix} 0 & -v^{b}_{rz} & v^{b}_{ry} & v^{b}_{x} \\ v^{b}_{rz} & 0 & -v^{b}_{rx} & v^{b}_{y} \\ -v^{b}_{ry} & v^{b}_{rx} & 0 & v^{b}_{z} \\ 0 & 0 & 0 & 0 \end{bmatrix}.
$$
If you have the joints follow the trajectory defined by
$$
\omega = J_{b}^{-1} \frac{v^{b}}{T}
$$
the end effector will move between the start and end configurations at a constant body velocity.

You can construct your Jacobian so that it gives the spatial velocity of the end effector (its velocity with respect to the world, measured as the velocity of the frame attached to the end effector, but currently overlapping the origin). If you have a world-frame Jacobian, you can get the spatial Jacobian by using the  end-effector's position as,
$$
J_{w} = \begin{bmatrix}
%
\begin{pmatrix} 1 &&\\ &1&\\ && 1 \end{pmatrix}
%
&
%
\begin{pmatrix} \phantom{-}0 & -z & \phantom{-}y\\ \phantom{-}z& \phantom{-}0  & -x \\ -y& \phantom{-}x & \phantom{-}0
\end{pmatrix} \\ \begin{pmatrix} 0&& \\ &0&\\ &&0 \end{pmatrix} & \begin{pmatrix} 1 &  &  \\  & 1 &  \\ && 1 \end{pmatrix} \end{bmatrix} J^{s},
$$
with the terms in the upper-right block encoding the cross-product that relates the translational velocities of attached frames.

Once you have done this, you can find the spatial transformation from $A$ to $B$ (the global transformation that takes $A$ to $B$) by right-multiplying $B$ by the inverse of $A$,
$$
\Delta^{s} = BA^{-1}.
$$
Taking the matrix log of $\Delta^{s}$,
$$
\delta^{s} = \log_{m}(\Delta^{s})
$$
gives you a matrix whose components are of the same form as those of $\delta^{b}$,
$$
\delta^{s} = \begin{bmatrix} 0 & -v^{s}_{rz} & v^{s}_{ry} & v^{s}_{x} \\ v^{s}_{rz} & 0 & -v^{s}_{rx} & v^{s}_{y} \\ -v^{s}_{ry} & v^{s}_{rx} & 0 & v^{s}_{z} \\ 0 & 0 & 0 & 0 \end{bmatrix}.
$$
If you have the joints follow the trajectory defined by
$$
\omega = J_{s}^{-1} \frac{v^{s}}{T}
$$
the end effector will move between the start and end configurations at a constant spatial velocity (i.e. the motion will be as if the end effector was extended to cover the whole space, and someone at the origin was propelling it at what they see as a constant velocity).

An interesting property of physical motions is that both 2 and 3 produce the same trajectory -- a helix around a fixed axis, with special cases of a planar arc when the rotation axis is perpendicular to the translation vector, and a straight line when there is no rotation.

For a smoother motion, you can use a time parameterization that changes the speed across the path so that it eases into and out of the motion.

Beyond the options described above, you start to get deep into motion planning, where you choose paths that minimize things like joint torques or payload acceleration/jerk, take into account the mass distribution of the payload, or include the masses of the links as well as the payload.

Title: Controlling the pose of a manipulator using inverse kinematics

Body:
I want to control a robotic manipulator in Cartesian space, using inverse kinematics. I know that I can do this using $\omega=J^{-1}v$, where $\omega$ is a vector of joint velocities, $J$ is the Jacobian, and $v$ is a vector of {position, orientation} velocities in Cartesian space. Here, the Jacobian expresses how the Cartesian {positions, orientations} change with respect to the joint angles.

My question is as follows. What I have, is two poses of the manipulator in Cartesian space, pose $A$ and pose $B$. And I want to move the manipulator from $A$ to $B$. How do I create the vector $v$ needed for the above inverse kinematics equation?

I am confused, because currently $A$ and $B$ are transformation matrices, expressing the pose of the manipulator in the fixed world coordinates. So, somehow I need to find the difference between the two poses, and then I need to convert this "difference" into the velocity vector $v$. Can anybody help me out please?


问:
Inverse Kinematics with Orientations
Let's say I have a robot consisting of a base $B$ and a manipulator $M$. The pose of the manipulator can be expressed as the transformation matrix $T_{BM}$. I also have a goal $G$ which I want to move the manipulator to, which can be expressed as the transformation matrix $T_{BG}$. My task is to compute the robot's joint angles $\theta$ which will set the manipulator to the goal pose.

To do this, I can use inverse kinematics. Now, I have some code which will take in the robot's URDF, and give me the Jacobian. This Jacobian tells me the rate of change of $M$ with respect to the rate of change of $\theta$. In other words, $J=\frac{d T_{BM}}{d \theta}$. And so to find the joint angles for the goal pose, I can use $d \theta = J^{-1} d T_{BM}$. This gives me the change in joint angles needed to reach the goal pose. (In practice, the Jacobian is just a local linearisation of the derivative, so I would perform this step multiple times).

My question is follows. The Jacobian is a 6-by-N matrix (for an N-DOF arm), and so I need a 6-by-1 vector to represent $d T_{BM}$. What should I use for $d T_{BM}$, given that all I know are the poses $T_{BM}$ and $T_{BG}$?

Here are my thoughts so far. For the linear position, it seems easy enough to just find the linear position difference between $T_{BM}$ and $T_{BG}$. But what I am confused about, is how to calculate the orientation component of $d T_{BM}$. The Jacobian defines the rate of change of rotation about the {x, y, z} axes, and so I need to express $d T_{BM}$ in terms of rotations about these axes. But how do I get this from $T_{BM}$ and $T_{BG}$? I know how to calculate $T_{MG}$ from these, and then convert the rotation into Euler angles. But it doesn't seem right that these Euler angles are what I should use for $d T_{BM}$, because the Jacobian expresses the instantaneous rate of change around all three axes simultaneously, whereas Euler angles describe a rotation by applying each of the rotations sequentially.

So, any help in understanding this would be great!

答:
The Jacobian is a 6x6 matrix and there are multiple ways it can be derived. The concept of the Jacobian has its roots in Mathematics, where it is not stricly tied to any problem, it is just a matrix of partial derivates of an equation system.

As such it is obtained as:

$$
J = \frac{\partial f}{\partial \theta} =
\begin{bmatrix}
  \frac{\partial f_{\text{1}}}{\partial \theta_1}, & \frac{\partial f_{\text{1}}}{\partial \theta_2} & ..., \frac{\partial f_{\text{1}}}{\partial \theta_n} \\
.\\
.\\
.\\
  \frac{\partial f_{\text{n}}}{\partial \theta_1}, & \frac{\partial f_{\text{n}}}{\partial \theta_2} & ..., \frac{\partial f_{\text{n}}}{\partial \theta_n}
\end{bmatrix}
$$

When definee like this:
$$ \dot X = J \times \dot Q$$
$\dot X = [\dot x, \dot y, \dot z, \dot A, \dot B, \dot C]^T$ and
$\dot Q = [\dot q_1, \dot q_2, \dot q_3, \dot q_4, \dot q_5, \dot q_6]^T$

There are other ways to obtain this matrix, but the other ways are just applying specifics of the robotics domain (to be more precise, specifics of a robot stucture in combination with specifics of the Denavit Hartenberg convention) to obtain the same result.

If we use the form above we can better identify the equations in question. $f_1$ represents the equation used to calculate the coordinate $x$ of the end-effector. it can be written as:

$$ x =f_1(\theta_1, \theta_2, ... \theta_n)$$

If we consider a 6 DOF robot

$$ x =f_1(\theta_1, \theta_2, ... \theta_6)$$
$$ y =f_2(\theta_1, \theta_2, ... \theta_6)$$
$$ z =f_3(\theta_1, \theta_2, ... \theta_6)$$
$$ A =f_4(\theta_1, \theta_2, ... \theta_6)$$
$$ B =f_5(\theta_1, \theta_2, ... \theta_6)$$
$$ C =f_6(\theta_1, \theta_2, ... \theta_6)$$

Now you can see from here, that these are the forward kinematics equations, and the Jacobi matrix obtained this way will be dependent on $\theta_1, ... \theta_6$. Furthermore, it can be observed, that these equations are in fact the ones which determine in which frame are the orientations expressed and in which order. Any combination is possible, in any order, as long as the mathematical equality stands. So it is important to notice that the concept of the Jacobi matrix itself is agnostic of how the rotations are expressed. It is the underlying equations which define the rotation order and the frame in which the rotations are expressed in.

Moving a robot can be very simple and very complex and anywhere in between.

Simplest is to use the inverse kinematics function, calculate the joint space coordinate of the target pose and use those as reference values for the position controller, no Jacobi involved, but it needs inverse kinematics.

The Jacobi matrix converts velocities, not position. In order to use it, one must  task space (Cartesian space) velocities and feed those to a velocity controller. Here care must be taken to end up with 0 velocity at the target point in order to stay at the target position. See the answers to this question for more details about this.

Title: Inverse Kinematics with Orientations


问:
Conveyor Belt Calibration for a Robot - Rotation Matrix
Short Question:

As demonstrated in the image below, how can I calculate the rotation matrix that relates the base coordinate and the rectangle coordinate, given 3 known points P1, P2, P3 on the surface of the rectangle with respect to the base coordinate?

Long Question:

I am currently working on OMRON pick-and-place robots. Basically, the overall layout of the system is similar to what is shown in the following picture:

.

The system has a rotary encoder that reflects the conveyor belt motion.

What I am trying to do is to come out with a transformation matrix T that represents the belt with respect to the robot coordinates. This transformation matrix is automatically calculated using a calibration wizard, however I am trying to understand how such matrix is calculated based on the 3-point belt calibration carried out as below:

Simple Calibration Steps:

The calibration can simply be completed after teaching 3 points on the belt as shown in the following image:

The steps are as follows:

A product is placed on the belt at the upstream limit that a robot
can start tracking the product from.

The robot tip is then placed on the product. Its location and the encoder value are recorded. Let the point be P1 and encoder value be E1.

The belt is then allowed to move in the product feeding direction until the it reaches to another location which the robot tip can reach.

Robot tip is placed on top of the product. This is the second calibration point P2. The encoder value is also recorded. Let it be E2.

Lastly the robot tip is moved and placed on the calibration product on a side location of the belt called downstream pick limit. This would be the the third point P3 and it is recorded.

After I followed these steps for a test robot in hand, I got the following quantities:

$$P_1 = \begin{bmatrix}-286.8 & -338.0 & 244.6\end{bmatrix} ; E_{1}=-71873$$

$$P_2 = \begin{bmatrix}140.3 & -422.2 & 246.7\end{bmatrix} ; E_{2}=-158710$$
$$P_3 = \begin{bmatrix}-18.004 & -590.452 & 248.746\end{bmatrix} $$

Let b = [bx by bz] be a belt point. Since the belt motion is having a magnitude along the x direction, then the scale factor in units of mm/pulses can be calculated by:

$$s = \frac {|P2-P1|}{E2-E1} = \frac {\sqrt{(140.3-(-286.8))^2 + (-422.2 -(-338.0))^2 + (246.7-244.6)^2}}{−158710 -(-71873)} =-0.005013$$

This is one important scale factor that we need to calculate during the calibration process (the minus sign means the encoder counts opposite to belt rotation)

Along with this scale that we get during calibration, we need to calculate all members of the transformation matrix T which relates belt coordinates with robot coordinates. This has the rotation matrix R and translation matrix D.
$$
p=Tb = \begin{bmatrix}& R & & D\\
0 & 0 & 0 & 1
\end{bmatrix}b
$$

$$p=\begin{bmatrix}
p_x \\
p_y\\
p_z\\
1
\end{bmatrix}
=
\begin{bmatrix}
r_{11} & r_{12} & r_{13} & d_{x}\\
r_{21} & r_{22} & r_{23} & d_{y}\\
r_{31} & r_{32} & r_{33} & d_{z}\\
0 & 0 & 0 & 1\\
\end{bmatrix}
\begin{bmatrix}
b_x \\
b_y\\
b_z\\
1
\end{bmatrix}
$$

Since b always has one component along the x axis of the belt, then the coordinate of the first calibration point with respect to belt coordinate is (Belt did not move yet):
$$
b_1 =
\begin{bmatrix}
0  \\
0\\
0\\
1
\end{bmatrix}
$$

and the coordinate of the second calibration point with respect to belt coordinate is (Belt moved from P1 to P2):

$$
b_2 =
\begin{bmatrix}
s\times(E2-E1)  \\
0\\
0\\
1
\end{bmatrix}
=
\begin{bmatrix}
435.3  \\
0\\
0\\
1
\end{bmatrix}
$$

Therefore, from P1 calibration, we have:

$$p_1=\begin{bmatrix}
−286.8 \\
−338.0\\
244.6\\
1
\end{bmatrix}
=
\begin{bmatrix}
r_{11} & r_{12} & r_{13} & d_{x}\\
r_{21} & r_{22} & r_{23} & d_{y}\\
r_{31} & r_{32} & r_{33} & d_{z}\\
0 & 0 & 0 & 1\\
\end{bmatrix}
\begin{bmatrix}
0 \\
0\\
0\\
1
\end{bmatrix}
$$

Solving for T matrix members, we get dx = −286.8, dy = −338.0 and dz = 244.6

From P2 calibration, we have:
$$p_2=\begin{bmatrix}
140.3  \\
−422.2\\
246.7\\
1
\end{bmatrix}
=
\begin{bmatrix}
r_{11} & r_{12} & r_{13} & −286.8\\
r_{21} & r_{22} & r_{23} & −338.0\\
r_{31} & r_{32} & r_{33} & 244.6\\
0 & 0 & 0 & 1\\
\end{bmatrix}
\begin{bmatrix}
435.3  \\
0\\
0\\
1
\end{bmatrix}
$$

Solving for the Rotation matrix, we get:
$$
R = \begin{bmatrix}
0.981112257 & r_{12} & r_{13}\\
-0.193378015 & r_{22} & r_{23}\\
0.004866365 & r_{32} & r_{33}
\end{bmatrix}
$$

Based on OMRON calibration wizard, the following matrix is obtained after finishing teaching the 3 points:
$$
T = \begin{bmatrix}
0.981112257 & -0.193427089 & 0.00216786 & −286.8\\
-0.193378015 & -0.981022085 & -0.01416372 & −338.0\\
0.004866365 & 0.013476983 & -0.999897339 & 244.6\\
0 & 0 & 0 & 1\\
\end{bmatrix}
$$

As seen, the first column and 4th conform with the results that we got. However, I am not really sure what the mathematical way to follow using the third calibration point to calculate the rest of the transformation matrix that I have. How would I be able to calculate r12, r22, r32, r13, r23 and r33?

-EDIT: (Response to the answer provided by @50k4)

I have tried your equations to find P4 for a sample coordinate that I got from the Google Sketchup 3D Designer, however I am getting big illogical values a and b and consequently for its x, y and z. I created a Google sheet which contains the design with assigned values of each point and I also implemented your equations in it to calculate a, b, and P4 as well as $\vec v_1 = P_3 - P_4$ and $\vec v_2 = P_1 - P_4$. Here is the link for this sheet that I shared and can be publicly edited:

https://docs.google.com/spreadsheets/d/1XGTqjk7kfwNi0hfktoVEtkkTNgrD1LUz8ui2Tyu455Y/edit?usp=sharing

Why I Need to Know This in The First Place!

I am an experienced software engineer with robotics and control engineering qualification. I am planning to program 3D models and create a simulation platform and solve a problem in hand for the robotics world. Clear understanding of this field in mathematics is required.

答:
A general approach would be to construct a so called axis-angle representation and convert that to a rotation matrix representation.

On order to do so, one could start with a normal vector of the triangle.

Let's take the $P_1P_2$ line and construct the perpendicular of the triangle from $P_3$ to $P_1P_2$. Let's call $P_4$ the point where the perpendicular intersects $P_1P_2$ .Based on this answer the coordinates of this point can be found as:

$\alpha = \frac{(x_3-x_1) \cdot (x_2-x_1) + (y_3-y_1) \cdot (y_2-y_1) + (z_3-z_1) \cdot (z_2-z_1)}  {(x_2-x_1) \cdot (x_2-x_1) + (y_2-y_1) \cdot (y_2-y_1) + (z_2-z_1) \cdot (z_2-z_1)}$

$P_4 = (x_1 + \alpha  \cdot (x_2-x_1), y_1+\alpha \cdot (y_2-y_1), z_1 + \alpha \cdot (z_2-z_1))$
where:

$P1 = (x_1, y_1, z_1)$

$P2 = (x_2, y_2, z_2)$

$P3 = (x_3, y_3, z_3)$

so $P_1P_2$ and $P_3P_4$ are perpendicular and their intersection point is $P_4$.

Applied to the coordinates described in the attached google sheet file the triangle (in blue) and the perpendicular in red is presented in the following figure:

We can now construct 2 perpendicular vectors, in the $P_1P_2P_3$ plane which share their origin in $P_4$

$\vec v_1 = P_3 - P_4$

$\vec v_2 = P_1 - P_4$ (we could also use $\vec v_2 = P_2 - P_4$, this would cause an 180 deg offset around the z axis)

The cross product of these two vectors will be the normal of the $P_1P_2P_3$ plane.

$\vec v_3 = \vec v_1 \times \vec v_2$

care must be taken to make sure that $\vec v_3$ points "upwards or downwards", depending on the cross product order ($\vec v_1 \times \vec v_2$ or $\vec v_2 \times \vec v_1$). The preferred one has to be chosen.

The three vectors now form an orthogonal coordinate system. in order to get the axes of this coordinate system the vectors should be normalized.

$\vec x_n = normalize(\vec v_2) = [x_{xn}, y_{xn}, z_{xn}]$

$\vec y_n = normalize(\vec v_1) = [x_{yn}, y_{yn}, z_{yn}]$

$\vec z_n = normalize(\vec v_3) = [x_{zn}, y_{zn}, z_{zn}]$

Please note that $\vec v_2$ has been chosen as $\vec x_n$ based on the additional information in the question, that $P_1P_2$ is parallel to the $X$ axis of the belt/rectangle frame.

A rotation matrix can also be interpreted as a direction cosine matrix, based in this, the rotation matrix of the plane can be written as:

$$
R = \begin{bmatrix} x_{xn} & y_{xn} &z_{xn} \\
 x_{yn} & y_{yn} & z_{yn} \\
 x_{zn} & y_{zn} & z_{zn} \end{bmatrix}.
$$


答:
Python 2.7 has been sunsetted.

From the official documentation:

What will happen if I did not upgrade by January 1st, 2020?

If people find catastrophic security problems in Python 2, or in software written in Python 2, then most volunteers will not help fix them. If you need help with Python 2 software, then many volunteers will not help you, and over time fewer and fewer volunteers will be able to help you. You will lose chances to use good tools because they will only run on Python 3, and you will slow down people who depend on you and work with you.

Some of these problems started on January 1. Other problems will grow over time.

(emphasis added)

Title: OpenCV4.2 and Ubuntu16.04 with python2.7?

Body:
Summary:

I'm using new vision of opencv, opencv 4.2 with ubuntu 16.04 and python 2.7

After built cv_bridge, the output is:

```<code>import rospkg
ImportError: No module named rospkg
</code>```

Here's the more detailed version: here

I tried to build cv_bridge in order to run OpenCV 4.2, Ubuntu 16.04 (ROS Kinetic) and python 2.7.

I will explain what I do for that.

I built it before with OpenCV 3.3.1 in order to use GPU and it was very fine and it is still working now. To best my knowledge, there is ros-kinetic-opencv3, it is working only on CPU. I removed it using sudo apt remove ros-kinetic-opencv3

After removing, I just downloaded your here and put in my ~/catkin_ws/src/, then making ~/catkin_ws/catkin_make on my PC without any modifications. I think that I was lucky.

Now, I'd like use some advantage of OpenCV 4.2 that is not with older versions like OpenCV 3.3.1. Meanwhile, I can't update robot's system, it is Ubuntu 16.04.

So, I tried to build cv_bridge, all modifications as following:

I copied and pasted module_opencv3.cpp, then change its name to module_opencv4.cpp so that became three models 4, 3 and 2.cpp instead of two 3 and 2.cpp.

Next , Sreevan mentioned here some changes, I did like Sreevan:

In cv_bridge/src/module_opencv3.cpp change signature of two functions

3.1) UMatData* allocate(int dims0, const int* sizes, int type, void* data, size_t* step, int flags, UMatUsageFlags usageFlags) const
to
UMatData* allocate(int dims0, const int* sizes, int type, void* data, size_t* step, AccessFlag flags, UMatUsageFlags usageFlags) const

3.2) bool allocate(UMatData* u, int accessFlags, UMatUsageFlags usageFlags) const
to
bool allocate(UMatData* u, AccessFlag accessFlags, UMatUsageFlags usageFlags) const

Next, I went to this file ~/catkin_ws/src/vision_opencv/cv_bridge$ CMakeLists.txt and change it as:
from:

```<code>find_package(OpenCV 3 REQUIRED
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  PATHS /usr/local
  NO_DEFAULT_PATH
  CONFIG
)
</code>```

to:

```<code>if ( NOT (OpenCV_VERSION_MAJOR VERSION_LESS 4) )
find_package(OpenCV REQUIRED
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  #PATHS /usr/local/include
  PATHS /usr/local
  NO_DEFAULT_PATH
  CONFIG
)
else()
find_package(OpenCV 3 REQUIRED
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  PATHS /usr/local
  NO_DEFAULT_PATH
  CONFIG
)
endif()
</code>```

Next, I went to this file ~/catkin_ws/src/vision_opencv/cv_bridge/src$ CMakeLists.txt and change it as:
from:

```<code>if (OpenCV_VERSION_MAJOR VERSION_EQUAL 3)
  add_library(<span class="math-container">${PROJECT_NAME}_boost module.cpp module_opencv3.cpp)
else()
  add_library($</span>{PROJECT_NAME}_boost module.cpp module_opencv2.cpp)
endif()
</code>```

to

```<code>if (OpenCV_VERSION_MAJOR VERSION_EQUAL 4)
  add_library(<span class="math-container">${PROJECT_NAME}_boost module.cpp module_opencv4.cpp)
elseif(OpenCV_VERSION_MAJOR VERSION_EQUAL 3)
  add_library($</span>{PROJECT_NAME}_boost module.cpp module_opencv3.cpp)
else()
  add_library(${PROJECT_NAME}_boost module.cpp module_opencv2.cpp)
endif()
</code>```

Next, I think this step is not important, I went to this file ~/catkin_ws/src/vision_opencv/cv_bridge$ package.xml and change any opencv3 to opencv4

Finally, ~/catkin_ws$ catkin_make

everything was ok but when I run the code:

```<code>import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

print(cv2.__version__)

class CVControl:
  def __init__(self):
    self.bridge = CvBridge()
    self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.img_callback)
  def img_callback(self, data):
    try:
      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print e

    frame = cv_image
    cv2.imshow("Image window", frame)
    cv2.waitKey(3)
def main():
  ctrl = CVControl()
  rospy.init_node('image_converter')
  try:
    rospy.spin()

  except KeyboardInterrupt:
    print "Shutting down"
    cv2.destroyAllWindows()

if __name__ == '__main__':
  main()
</code>```

It runs by OpenCV 3.3.1

If I need to run it by OpenCV 4.2
I added those lines in first the code:

```<code>import sys
ros_path2 = '/usr/local/lib/python2.7/site-packages'
ros_path3 = '/usr/lib/python2.7/dist-packages'
if  ros_path2 and ros_path3 in sys.path:
  sys.path.remove(ros_path2)
  sys.path.remove(ros_path3)
</code>```

The output is:

```<code>import rospkg
ImportError: No module named rospkg
</code>```

What is wrong?


问:
Jacobian to determine the linear velocity of each node (DOF 3 RRR robot arm)
I've done the FK and IK for this robot:

On those I seem to be able to get to the positions I want on the simulator so I'm assuming nothing is wrong with my FK and IK calculations. I've tried to calculate the Jacobian to later determine the speed at which each actuator needs to go while inputting a linear speed for the endpoint. I assume I can calculate that by inverting the Jacobian matrix and multiplying it to the speed I want, getting a 1x3 matrix with the speeds for each joint.
However, the Jacobian matrix I get isn't square (6x3) and even if I only consider the linear speed part (top part which is square), that part isn't possible to invert (rank of it is lower than it's length).

Here's the parameters that constitute the Jacobian matrix that I calculated:

$$Z^0_0 = \begin{bmatrix}0  \\  0  \\  1\end{bmatrix}$$

$$Z^0_1 = \begin{bmatrix}\sin(\theta_1)  \\  -\cos(\theta_1)  \\  0\end{bmatrix}$$

$$Z^0_2 = \begin{bmatrix}\cos(\theta_1)*\sin(\theta_2)+\cos(\theta_2)*\sin(\theta_1)  \\  \sin(\theta_1)*\sin(\theta_2)-\cos(\theta_1)*\cos(\theta_2)  \\  0\end{bmatrix}$$

$$O^0_0 = \begin{bmatrix}0 \\ 0  \\  0\end{bmatrix}$$

$$O^0_1 = \begin{bmatrix}0  \\  0  \\  d_1\end{bmatrix}$$

$$O^0_2 = \begin{bmatrix}d_2*\cos(\theta_2)  \\  d_2*\sin(\theta_2)  \\  d_1\end{bmatrix}$$

$$O^0_3 = \begin{bmatrix}d_3*\cos(\theta_3)+d_2*\cos(\theta_2)*\cos(\theta_3)-d_2*\sin(\theta_2)*\sin(\theta_3)  \\  d_3*\sin(\theta_3)+d_2*\cos(\theta_2)*\sin(\theta_3)+d_2*\cos(\theta_3)*\sin(\theta_2)  \\  d_1\end{bmatrix}$$

$$J = \begin{bmatrix} Z^0_0\times(O^0_3 - O^0_0) & Z^0_1\times(O^0_3 - O^0_1) & Z^0_2\times(O^0_3 - O^0_2) \\ Z^0_0 & Z^0_1 & Z^0_2\end{bmatrix}$$

$$ \dot{q}(t) = \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \dot{q_3} \end{bmatrix}$$

What am I doing wrong?

EDIT: Added the DH parameters and used MathJax to better show the matrixes.

\begin{array} {|r|r|}\hline link & a_i  & \alpha_i & d_i & \theta_i \\ \hline 1 & 0 & -90º & d1 & \theta_1* \\ \hline 2 & d2 & 0 & 0 & \theta_2* \\ \hline 3 & d3 & 0 & 0 & \theta_3* \\ \hline  \end{array}

EDIT2: Added the transformation matrixes for each joint.

$$ H^1_0 = \begin{bmatrix} \cos(\theta_1) & 0 & \sin(\theta_1) & 0 \\ \sin(\theta_1) & 0 & -\cos(\theta_1) & 0 \\ 0 & 1 & 0 & d_1 \\ 0 & 0 & 0 & 1\end{bmatrix} $$

$$ H^2_1 = \begin{bmatrix} \cos(\theta_2) & -\sin(\theta_2) & 0 & d_2*\cos(\theta_2) \\ \sin(\theta_2) & \cos(\theta_2) & 0 & d_2*\sin(\theta_2) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix} $$

$$ H^3_2 = \begin{bmatrix} \cos(\theta_3) & -\sin(\theta_3) & 0 & d_3*\cos(\theta_3) \\ \sin(\theta_3) & \cos(\theta_3) & 0 & d_3*\sin(\theta_3) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix} $$

$$ H^2_0 = H^2_1 * H^1_0  $$
$$ H^3_0 = H^3_2 * H^2_1 * H^1_0 $$

From $ H^n_0 $ I'm getting $ Z_n $ and $ O^n_0 $ which are:

$$Z_n =\begin{bmatrix}H^n_0(1,3) \\ H^n_0(2,3) \\ H^n_0(3,3) \end{bmatrix} , O^n_0 = \begin{bmatrix}H^n_0(1,4) \\ H^n_0(2,4) \\ H^n_0(3,4) \end{bmatrix}$$

答:
Your axes and link vectors seem off. The axes $z_{1}$ and $z_{2}$ should be parallel, and your $O^{0}_{2}$ and $O^{0}_{3}$ don’t seem to be accounting for all the rotations that occur prior to the link.

Note that as drawn, your arm is in a singular configuration. Shoulder and elbow will both move the end effector up, but no joint can extend it further (because the elbow is hyperextended, placing the forearm inline with the upper arm). This means that if you try taking the inverse of the position block of the Jacobian, you’ll find it to be rank deficient.

———

Edit: I’m not a fan of DH for this. A better procedure would be:

Identify your reference configuration.

Take the local definition for each joint axis as its axis direction in the reference configuration.

Take the local link vector as its vector in the reference configuration.

Convert each rotation axis into a rotation matrix. If you’ve chosen a good reference configuration, the resulting matrices will be canonical $R_x$, $R_{y}$, or $R_{z}$ matrices.

Get the net rotations along the chain by taking the cumulative product of the rotation matrices working left to right as you go down the chain.

Get the link vectors by multiplying the net rotations into the corresponding local link vectors.

Get the link endpoints by taking the cumulative sum of the link vectors.

Get the rotation axes by multiplying the net rotations into the corresponding local link axes.

If you want to get one step fancier, you can build joint and link transformation matrices by augmenting the rotation matrices with a zero translation vector and the link transformation matrices with an identity rotation matrix. You can then take the left-to-right cumulative product to get the transform to each point on the chain.


问:
Where do matrix A and A transpose come from in calculating the predicted covariance matrix?
I don't understand where the matrices A and A transpose come from in the equation in this series. I have done a one-dimensional example in the series but I don't see how to derive these matrices in matrix form.

答:
The $A$ matrix (sometimes $F$) comes from your equations of motion. There's a technical example on Wikipedia that goes over modeling position and speed of a truck.

You can model speed and position with the following equations:

$$
x = x_0 + \Delta t \dot{x} + \frac{1}{2}\Delta t^2 \ddot{x} \\
\dot{x} = \dot{x}_0 + \Delta t \ddot{x} \\
$$

Assuming your input $u$ is the acceleration ($u = \ddot{x}$), and being more explicit with the coefficients on everything, you could rewrite the above as:

$$
x = \left(1\right)x_0 + \left(\Delta t\right) \dot{x} + \left(\frac{1}{2}\Delta t^2\right) u \\
\dot{x} = \left(0\right)x_0 + \left(1\right)\dot{x}_0 + \left(\Delta t\right)u \\
$$

And then you convert to matrix form:

$$
\left[\begin{matrix}
x \\
\dot{x}\end{matrix}\right] =
\left[\begin{matrix}
1 & \Delta t \\
0 & 1 \end{matrix}\right]
\left[\begin{matrix}
x_0 \\
\dot{x}_0 \end{matrix}\right] + \left[\begin{matrix}
\frac{1}{2}\Delta t^2 \\
\Delta t\end{matrix}\right]u
$$

The current sample being:
$$
x_k = \left[\begin{matrix}
x \\
\dot{x}\end{matrix}\right]
$$
and the previous sample being:
$$
x_{k-1} = \left[\begin{matrix}
x_0 \\
\dot{x}_0 \end{matrix}\right]
$$

You can then define your "state matrix" $A$ or $F$ as:

$$
A =
\left[\begin{matrix}
1 & \Delta t \\
0 & 1 \end{matrix}\right]
$$
and your input matrix $B$ or $G$ as:

$$
B = \left[\begin{matrix}
\frac{1}{2}\Delta t^2 \\
\Delta t\end{matrix}\right]
$$

And so you have:

$$
x_k = Ax_{k-1} + Bu
$$

And then those are used with the output equations:

$$
y_k = Cx_k + Du
$$

to form the Kalman filter. These are all standard matrices from the state space representation of a system.


问:
Difference between constant curvature kinematics and rigid link kinematics
What is the difference between constant curvature kinematics and rigid link kinematics and what is their respective definition?</p>

答:
Rigid link kinematics assumes that the robot is made of rigid structural elements connected by pivot or sliding joints, and describes the locations of points on the robot (or more generally, the locations and orientations of frames attached to the robot) as a function of the joint angles.

Constant-curvature kinematics assumes that the robot is made up of a series of segments that can bend into constant-curvature (i.e., circular) arcs, and describes the locations of points (or locations and orientations of frames) as a function of the segment curvatures.


问:
ROS Network configuration on multiple machines - Publisher and Subsciber
I am trying to set up a simple and listener talker across two machines connected on the same wifi network.
Machine A has the 10.42.0.89 address and is running the roscore and another node that publishes messages on a topic.

```<code>$ printenv | grep ROS
ROS_IP=10.42.0.89
ROS_MASTER_URI=http://10.42.0.89:11311
</code>```

The other Machine B has the ROS_MASTER_URI set to match the one of machine A

```<code>$ printenv | grep ROS
ROS_MASTER_URI=http://10.42.0.89:11311
</code>```

Machine B can get the list of available topics and running nodes correctly

```<code>$ rostopic list
/pose/estimate
</code>```

But can't print out messages in that topic using rostopic echo /pose/estimate.

Do you have any advice?

答: I had this problem and I solved my problem by adding both machines' hostname to /etc/hosts. refer to https://husarion.com/tutorials/ros-tutorials/5-running-ros-on-multiple-machines/</p>


问:
How do you write unit tests for robotics related software?
In my case, I have a library handling 3D input data and want to write tests in order to make sure that my coordinate transform chains for getting the 3D points from A to B are correct.

The problem I see here is that in order to write a test, I have to reimplement the coordinate transformation on the test level and compare its result to what my code does. Isn't that a chicken an egg problem? What tells me that my implementation in my testing is more correct than the code itself?

答:
Your statement about the need to reimplement the same feature in order to test it could be applied for all software which needs to be unit-tested and it is not specific only to your problem. A unit test does not implement redundant functionality, in this case an error cannot be attributed to a faulty implementation in the tested code, as it can just as easily be a faulty implementation of the test code.

Unit tests for transformations usually work with data where you can devise the correct output for a given input by other means that the currently implemented software which needs testing. Ways to do this are:

Fringe cases like zero transformations where the output is the same as the input

Cases where the answer is easily predictable, like 90 (180, 270, 360, -90, -180, -360) degrees rotations without translations or simple translations without rotations

Back-and forth transformation chains: define a complex chain and define the exact opposite transformations and at the end you have to end up in the starting point

Compound transformation chains: e.g. two consecutive rotations around the same axis should be equal to one rotation around the same axis with the compounded magnitude, or more complex compound operations can be also defined.

Compare against mock data originating from other systems: create a dataset in Matlab, Octave, a CAD Software package or any other trusted source which includes the origin the transformation and the transformed entity. Save these to a file, read them in for the test and compare the results. (you could do also connect to such software and execute the transinformation, but that would introduce unnecessary dependencies in your tests)

Carefully hand-craft a "master" or "golden" dataset for validation, same as above, but the file is created manually.

Probably a combination of the above will lead to good results, but these test should be treated with an appropriate amount of scepticism.


问:
Dexterity calculation with dual quaternions
How can the manipulability of a robot arm be calculated when the kinematics are described with screw theory such as dual quaternions?  These methods are sigularity-free, but what could we say about dexterity?</p>

答:
A robot arm's dexterity is a function of its physical structure (arrangement of joints) and configuration (joint positions).  And it can change depending on the way the arm is positioned.  It has nothing to do with the mathematical representation of the arm (DH parameters vs. dual quaternions, etc).

Looking at the function for manipulability involves the Jacobian.  But since this must be computed at specific joint angles, the Jacobian is just a matrix of numbers:

$$
\mathit{w}(q) = \sqrt{det(\mathbf{J}(q)\mathbf{J}^T(q)}
$$

As you said, this dexterity can be calculated with a "manipulability" score.  But this is just a metric.  And it is not the only one.  I discuss a few other metrics in my answer to this post: Evaluating the similarity of two 7 Degree of Freedom Arms

And for an example of how different arrangements of joints can lead to different manipulabilty scores, see my answer in this post: How do you arrange the dof in a robotic arm?


问:
My Raspberry Pi is losing power in a surge
I have an RC car. The battery provides power to the ESC and then the ESC provides 6 V back out to the receiver. Instead of the receiver I have a Raspberry Pi, which uses the 6 V, steps it down to 5 V and provides power to the Raspberry Pi.

The problem

Every time we go full power*, there is a lack of voltage and the Raspberry Pi seems to hard reset.

* By full power we mean direct to 100% and not ranging from 0-100

I am not an expert in electrical circuits, but some of the suggestions are to use a capacitor to provide the missing 5 V in the interim. How do I prevent the Raspberry Pi from dying in the event of full power?

答:
I must agree with the other two answers, however the main issue is that you do not have enough voltage into your regulator (I see from your comment to Ian that you are using a Pololu D15V35F5S3 Regulator). If you refer to the Pololu D15V35F5S3 Product Description, down at the bottom you will find the following graph:

Looking at the red line for 5V output: Note for all currents greater than zero, the dropout voltage is greater than 1V. (The minimum input voltage necessary to achieve 5V output is 5V + dropout voltage.) The more current used by your 5V loads (Pi), the greater the dropout. The problem is compounded by any voltage drop in your 6V source due to current surges (see Ian's answer).

You either need a higher input voltage, a lower dropout regulator (this may be difficult and insufficient), a different regulator (buck-boost), or a different power source for the Pi.

ContentLicense: CC BY-SA 3.0


问:
Can ROS be an alternative for a KFLOP or RUMBA Board?
I have a few questions about controlling robot arm motion. I see that some have used Dynomotion KFLOP and other CNC motion controllers to control arms. Some have used Rumba board as well as several thousand dollar boards. But why don't we all just use ROS? What are the main benefits these boards give? AFAIK neither the KFLOP or Rumba allow realtime control. How come people invest (time/money) into these boards rather than ROS?</p>

答:
You are comparing different products made for different use-cases.

ROS is an open source software solution to control robots. It is intended (mainly) to be programmed by highly qualified software developers and in some rare cases the robots running ROS are used in the industry also. There is no technical support whatsoever, there are no guarantees that the software is capable of doing what you are planning to do. In order to control a robot from ROS you need a PC (or some similar iPC or SBC) and you need additional hardware to drive a motor (an amplifier stage and a motor controller). The drive amplifier has to be compatible with the motor and has to have a ROS driver.

CNC motion controllers are professional industrial equipment controlling machine tools in the industry. Modern CNC controllers are PC based, earlier ones had proprietary hardware. Normally, a drive amplifier is connected to the CNC controller in order to be able to drive a motor (and subsequently a machine tool). Furthermore a CNC acts like a firmware, which allows you to load and execute application programs (the so called G-Code) which are workpiece specific. There are companies like Siemens or Fanuc who offer high-end CNC controllers and others who offer compatible drive amplifiers and offer technical support, warranty, certification etc. Professional CNC controllers are not used to control robot arms, these are used for machine tools. Robot controllers are used to control robot arms. There are some subtle but important differences between robot controllers and CNCs.

The KFLOP board is a commercial product, which has tech support, warranties and required certifications (e.g. UL or CE certification). It seems that it takes a buffer of planned trajectories and executes this trajectory in real time (in contrary to ROS) and can be used as a motion controller (e.g. you specify a trajectory) or as a CNC controller (you specify G-Code or NC Code). ROS does not execute G-Code or NC code and does not offer real-time execution of a buffered trajectory. It seems that the KFLOP boards, like a PC running ROS,  also need additional hardware to connect it to motors.

The Rumba board is a low-cost solution to connect motors to your PC. It does not necessarily process trajectories, it is more similar to a drive amplifier as to the above 3 products. It is not real-time on the USB communication side, but depending on its internal software architecture it may offer a real-time (closed-loop) controller for the motors.

People usually invest money in commercial products because they see a benefit. this can be a functional or a non functional benefit. Commercial products offer tech support, warranty and sometimes certifications. This is not the case for open source products. ROS is software only, so you still need HW to run it, furthermore you need to connect the HW running ROS to the motors. The software is free but the hardware is not.

You mention that people invest time and money. That is usually a trade-off. You can invest a lot of time in learning ROS alone, or you can invest money and a very limited amount of time to buy a commercial product and a crash-course for that product and start using it significantly faster.


问:
Name of imitating motion technique in Robotics
This is my first question here and I'm not into this thing at all.

I recently saw a movie named Pacific Rim and in that I saw a technique in which a robotic arm imitates human arm(in the beginning of the movie). I was wondering what is that technique called and is it ever possible to build giant robots like the ones shown in the movie( Considering we've no shortage of budget).

I'm not into robotics at all so I just thought about asking it here.

Thank You!

答:
This is reference tracking. It's basically the sole point of the entire field of control theory.

Provide a reference input (like the encoder position from a device attached to your arm) and drive the system until the actual output matches the reference input.

Voltage regulation, cruise control, rocket guidance, autopilot, self-driving cars... nearly everything that has a "behavior" has a target they're trying to achieve.

And for giant robot arms specifically, there is the Hand of Man.


答:
Anthropomorphic or human like arms are often use in automation. Most 6 and 7 axis industrial robot arms are designed to move like human arms. We even use human terms like shoulder, elbow, forearm and wrist to describe them. </p>

<p>Traditionally a series of positions was taught to the robot which it then repeats. A few people are workings on methods of videoing a human doing a job and then translating that video in robot positions and motion. I can't say much more, other then it is not the company I work for, but rather some people that I have talked to. Some day there may be a actual product that people can see. </p>

Title: Name of imitating motion technique in Robotics

Body:
This is my first question here and I'm not into this thing at all.

I recently saw a movie named Pacific Rim and in that I saw a technique in which a robotic arm imitates human arm(in the beginning of the movie). I was wondering what is that technique called and is it ever possible to build giant robots like the ones shown in the movie( Considering we've no shortage of budget).

I'm not into robotics at all so I just thought about asking it here.

Thank You!

Body:
Anthropomorphic or human like arms are often use in automation. Most 6 and 7 axis industrial robot arms are designed to move like human arms. We even use human terms like shoulder, elbow, forearm and wrist to describe them.

Traditionally a series of positions was taught to the robot which it then repeats. A few people are workings on methods of videoing a human doing a job and then translating that video in robot positions and motion. I can't say much more, other then it is not the company I work for, but rather some people that I have talked to. Some day there may be a actual product that people can see.


答:
A robot manipulator that’s designed to mimic an operator in real-time can be called a “waldo” or "telemanipulator". The term "waldo" originates from Robert Heinlein, and was widespread enough at least in the science fiction literature to be used without explanation or comment as a technical term by characters in works such as The Forever War (1974) and Burning Chrome (1982).

The background section of this paper about the da Vinci surgical robot describes the history of the term as

Robert A. Heinlein's 1942 science fiction short story, titled "Waldo", described a glove and harness device that allowed the lead character, Waldo Farthingwaite-Jones – born frail and weak, and unable to lift his own body weight – to control a powerful mechanical arm by merely moving his hand and fingers. It was not long before these kinds of remote manipulators – popularly known as "waldoes" – were developed in the real world for moving and manipulating hazardous radioactive materials.


问:
ROS2 and TypeError when publishing custom message to Topic
I have defined a custom message: uint8[] data

The custom message is imported in my Node class with no problems:

```<code>from my_shared.msg import MyMessage
</code>```

In the same Node, I create the publisher with:

self.my_publisher = self.create_publisher(MyMessage, 'topic_in', 200)

and I publish the message with:

```<code>self.my_publisher.publish(my_msg)
</code>```

my_msg is built in the following way:

```<code>payload_bitstream = np.fromstring(my_data, np.uint8) my_msg = payload_bitstream.tolist()
</code>```

Sadly, I get a TypeError:

File "/opt/ros/eloquent/lib/python3.6/site-packages/rclpy/publisher.py", line 68, in publish raise TypeError() TypeError

Could you help out with this if you know what I am doing wrong pls?

Thanks in advance, G.

答:
The problem was caused by how I was creating and assigning value to the message.

The right way in ROS2 is:

```<code>my_msg = MyMessage()

...

my_msg.data = my_value
</code>```

that did the trick and solved my problem

Body:
The problem was caused by how I was creating and assigning value to the message.

The right way in ROS2 is:

my_msg = MyMessage()

...

my_msg.data = my_value

that did the trick and solved my problem


问:
Creating a fast, uniform, linear actuator
Most of the linear actuators I've seen are nonuniform and/or slow. Those using a cam or crankshaft-like mechanism (and nearly anything hydraulic/pneumatic) cannot be moved at a constant speed without some programming. Those using a screw-like mechanism are uniform, but slow.

Aside from a rack and pinion/rope wound around a stick, what other fast, uniform linear actuators exist? By uniform, I mean that the speed is uniform (Or the distance moved is linearly dependant on the angle rotated by the motor)

答:
It would be easier to give an answer if we knew what you want to use this for. That would give an idea of your requirements in terms of speed, cost, and space requirements.

As Rocketmagnet has pointed out, you can use a belt drive, or long pitch screw. And you have already mentioned a rack and pinion system. These are all involve a conventional (DC) motor along with a mechanical system to convert angular motion into linear motion:

belt drive

screw

rack & pinion

rigid chain actuator (this is also cool, it can be folded into a small space)

Other methods include:

Linear motor - This is quite cool, it involves a line of magnetic poles, which accelerate the rotor in a linear direction. These can be very fast and precise! This can also be controlled in open-loop (like a stepper motor).

Above is a cross-section of a linear motor

Pneumatic cylinder - the speed can be controlled using a control valve, pneumatic actuators are also quite popular for robotics applications because of its compliance.

Hydraulic cylinder

Wikipedia has an article on linear actuators, and also a comparison table with advantages and disadvantages!


问:
Particle filter implementation in ROS
I'm looking for particle filter implementation in ROS to use in mobile robot localization, but it seems the only available package is amcl (Adaptive Monte Carlo), I'm not sure is it possible to use it as particle filter or not, and if it's feasible, how?

Note: The robot (wheeled robot) provides odometry data and another data source is Kinect, that provides visual odometry data using fovis.

答:
Monte Carlo localization is just another name for a particle filter. Monte Carlo methods are a broader name for computational algorithms that rely on random sampling. A particle filter is a specific application of the general Monte Carlo method for localization, and so it is simply referred to sometimes as Monte Carlo localization.

If you ask Lord Google, he'll tell you everything you want to know:
http://en.wikipedia.org/wiki/Monte_Carlo_localization

Sebastian Thrun has a great book called Probabilistic Robotics that talks about MCL (Monte Carlo Localization) if you're interested in something more formal.

Edit: I just read the amcl rosnode wiki page and it actually mentions pretty much exactly what I've said here...I'm sure if you just read the node's wiki page you'll find your answers.

The amcl rosnode currently works with laser range data (i.e. from a LIDAR), so you'll have to extend it if you want to take advantage of a Kinect. It seems like you'll need some form of sensor measurement, as opposed to pure odometry. You could combine the odometry readings from the wheels & the visual odometry in a Kalman Filter, but with no sensor readings to update your position and fix errors based on a map, you'll just accumulate error and you'd have no sensor update in your MCL. You should be able to generate range data using the Kinect for the purpose of applying it as the sensor update in the MCL algorithm, and it might even be possible to publish this as laser data in ROS if you reformat it. This would allow you to interface directly with the existing amcl package.

Title: Particle filter implementation in ROS


问:
Design optimization method using indices
There are several methods like manipulability and conditioning index for design optimization kinematics performance, but these methods rely on the singular values of the jacobian matrix. I described kinematics by screw theory using dual quaternion that is singularity-free. I need an index to optimize links length and think, I shouldn't use manipulability and conditioning index for my purpose. What do you think???</p>

答:
I think it is likely that you are confusing two uses of the word “singular”:

The singular values of a matrix as found via singular value decomposition.

The singular configurations (or singularities) that occur when the Jacobian loses rank.


问:
Cartesian Angular Velocity
Let's say I have a Jacobian which gives the relationship between the velocity of a robot's joints, and the Cartesian velocity of its end-effector, relative to the end-effector's local frame. And let's say I also have a target pose to move the end-effector to, expressed relative to the end-effector's local frame, in Euler angles (a, b, c).

I want to move the end-effector to this target pose, and I want this rotation to happen in 1 second. To achieve this, I need to calculate the Cartesian velocity of the end-effector, so that I can use inverse kinematics to calculate the required joint velocities.

My first idea is to say that the Cartesian velocity is equal to the Euler angles (a, b, c), divided by the time (which is 1). So the angular velocity around the x-axis is a, the angular velocity around the y-axis is b, and the angular velocity around the z-axis is c.

After doing some reading, I'm not sure whether this is correct, and even if it is correct, if it is optimal.

Below, I have written three different things that might happen if I were to implement my controller like this. Which of these three is correct?

1) The end-effector rotates to the target pose in a smooth line (the most direct path).

2) The end-effector rotates to the target pose, but the motion is not smooth (it does not follow the most direct path).

3) The end-effector rotates but it does not end up at the target pose.

答:
Using Euler angles and the velocities expressed as a difference between Euler angles over time will work, but it does not give you the minimum distance between the angles.

If you calculate the velocity once like this, the end effector will move towards your goal. If you actually want to stop at your goal you will need to periodically recalculate velocities and times.

The questions you formulated are not easy to adress:

1) The end-effector rotates to the target pose in a smooth line (the
  most direct path).

Smooth does not mean "most direct". Smooth is defined in terms of discontinuities in the derivates of the position (i.e. velocity, acceleration and jerk). The trajectories obtained this way will not be smooth! They will have discontinuities already at the velocity level.
It is not clear what exactly do you mean my most direct. It can mean shortest trajectory in joint space or in Cartesian space. Rotations are expressed in a somewhat special coordinate system. More about this here. (Ironically this question was asked years ago by me). So to answer your question, the velocities you obtain this way will take you to the desired position and orientation, but they will not be smooth and they will not be the shortest path (for the orientation)

2) The end-effector rotates to the target pose, but the motion is not
  smooth (it does not follow the most direct path).

This is correct, if the velocity is recalculated periodically, based on current position. The velocity profile will not be smooth and the path will not be the shortest. Setting the velocity once means that robot will try follow this direction until a joint limit is reached, not until the target is reached. Furthermore s velocity is not reached instantaneously an different joints have different acceleration times, setting the velocity only once will result in a slightly different direction then intended.

3) The end-effector rotates but it does not end up at the target pose.

This depends on the way you control the velocity over time. It is expected that in different positions you will need different velocities. The absolute minium is that you stop the robot at the targer, better would be to slow it down when it is close to the target (but then the travel time would be longer, so you have to plan for this in advance).

Title: Cartesian Angular Velocity


答: I'd call it a waldo arm.  Another common term for it is a master-slave.</p>


问:
Compute path in absolute frame from sequence of measurements
I have a sequence of measurements $\{(x_i, y_i, \theta_i)\}_{i = 1 \:...\: N}$. Each of these represents the new pose of the robot seen from the previous pose, i.e. a measurement $(x', y', \theta')$ means that the robot moved of $x'$ and $y'$ units in the $x$ and $y$ axis of the previous coordinates system and rotated of an angle $\theta'$ afterward. Therefore, the value of $\theta$ represents the rotation of the new coordinates system w.r.t. the previous.

How can I compute the absolute path (the sequence of all poses expressed in the absolute frame)?

答:
Each step can be represented by its transformation matrix,
$$
\begin{bmatrix}
\cos{\theta'_{i}} & -\sin{\theta'_{i}} & x'_{i}\\
\sin{\theta'_{i}} & \phantom{-}\cos{\theta'_{i}} & y'_{i} \\
0 & 0 & 1
\end{bmatrix}.
$$
The world position at each step $i$ is the rightward-propagating product of the transforms up to that point,
$$
\begin{bmatrix}
\cos{\theta_{i}} & -\sin{\theta_{i}} & x_{i}\\
\sin{\theta_{i}} & \phantom{-}\cos{\theta_{i}} & y_{i} \\
0 & 0 & 1
\end{bmatrix}
=
\prod_{1}^{i}
\begin{bmatrix}
\cos{\theta'_{i}} & -\sin{\theta'_{i}} & x'_{i}\\
\sin{\theta'_{i}} & \phantom{-}\cos{\theta'_{i}} & y'_{i} \\
0 & 0 & 1
\end{bmatrix}.
$$

(This is the same math as is used for the forward kinematics of a robot arm -- each step is a link with geometry $x',y'$ followed by a joint rotation $\theta'$.)


答:
Assuming frame $0$ is the 'absolute frame', if we let $^j P_i$ be the $i^{th}$ position/orientation expressed in the $j^{th}$ coordinate frame, then what you're asking for is the sequence
$$\{(^0P_i)_{i=1 ... N}\},$$
correct?

Using your sequence of measurements $\{(x_i, y_i, \theta_i)_{i=1 ... N}\}$, it's straightforward to compute the $i^{th}$ 2-D transformation matrix,

$$^i_{i+1} T = \begin{pmatrix}
\cos \theta_i & -\sin \theta_i & x_i\\
\sin \theta_i & \cos \theta_i & y_i \\
0 & 0 & 1
\end{pmatrix}$$

And since you have all of them, use the fact that
$$^0_i T = ^0_1T ^1_2 T ... ^{i-1}_i T,$$ then just iterate through to list to compute each translation matrix and compute the data you're interested in. Note that you have to append a $1$ to each $P_i$ to make the 3x3 multiplcations work out (as noted in the reference):

$$^0P_{i+1} = ^0_i T ^i P_{i+1}$$

Hopefully that's helpful. Here's a video explaining frame transformations, that I often refer back to. It's really easy to get lost in notation.

EDIT: To compute the orientation of the $i^{th}$ coordinate, you can use the rotational component of the $^0_iT$ translation matrix, since you have the cos and sin of the angle in entries (1,1) and (2,1), just use the atan2 function to pull out $\theta$. Make sense?

Title: Compute path in absolute frame from sequence of measurements

Body:
I have a sequence of measurements $\{(x_i, y_i, \theta_i)\}_{i = 1 \:...\: N}$. Each of these represents the new pose of the robot seen from the previous pose, i.e. a measurement $(x', y', \theta')$ means that the robot moved of $x'$ and $y'$ units in the $x$ and $y$ axis of the previous coordinates system and rotated of an angle $\theta'$ afterward. Therefore, the value of $\theta$ represents the rotation of the new coordinates system w.r.t. the previous.

How can I compute the absolute path (the sequence of all poses expressed in the absolute frame)?


问:
Arduino Uno R3 or Roboduino ATMega168 or Arduino Mega 2560 R3 which board is better for small robots
I am new in robotics. May be this question looks like too naive but i want to know which is a better board among Arduino Uno R3 or Roboduino ATMega168 or Arduino Mega 2560 R3 for my purpose mention below -

A simple robot with wheels, can move around.

Can have IR sensors and camera.

Is powerful enough to deal with computer vision computations.

Arduino Mega 2560 R3 looks more powerful than the other too, I just want to know if my purpose can be solved with other two boards too?

Thanks

答:
Choosing which board/chip to use would also depend on the number of Input/Output ports you are going to need.

Uno has analogue 6 in / 0 out, digital IO 14/ 6 do PWM, 1 serial port (tx,rx)

Mega 2560 has 16 analogue / 0 out ports, digital IO 54/ 15 do PWM, 4 serial ports.

The robodunio ATMega168 has the same spec as the UNO but the clock speed is lower; It's also based upon the freedunio.

http://arduino.cc/en/Products.Compare

You will find it hard to process video feed with each of these chips. You could process the video on a Pi as all ready suggested or use other hardware to to process the video feed then pass the resulting information to the arduino's to enable movement processing or some other action needed.


问:
How to calculate the pitch of the quadcopter rotor blade and their efficiency?
I have gone to some of the blade designs and profiles of the blade which suits my requirement. But, once after entering the Theoretical calculation, I was stuck in the determination or the Justification for the Magnitude of Pitch mentioned in the Blade.

Example: I have taken the propeller of size 10 inch and 4.5-inch pitch value, Some blogs suggested to have a pitch of 0.2 times of the propeller dia for stable and efficient flight. But they didn't reveal how it affected the efficiency or why they stated those values...?

答: Not long ago I had the same problem of evaluating different blade designs. Well it's  not an easy task. I used JavaProp which is a tool for evaluating propeller designs. The author has an phd in aerdynamics so I guess the tool works properly. It allows you to caluclate the efficency depending on the pitch angles, altitude, cruising speed, rpm, motor torque etc.</p>


问:
Bias correction for multiple sensor fusion through Kalman Filtering
I am learning Kalman Filters and was working on a simple example:

Temperature measurement of a room by using 4 thermometers(different biases and noises)

if i consider that there is no bias in the system the model becomes simple with

x = [temp]
F = [1]; H = [1 1 1 1];

but lets assume my thermometers have a bias, how do I incorporate that into my model? How does my x, F and H matrix change?
I am assuming x = [x, B1, B2, B3, B4] but what becomes of F and H?

Here is the code i have written so far.

```<code>%a simple kalman test for multiple thermometers reading temp of same item
clc;clear;

N1 = 1;
N2 = 0.5
N3 = 1.5
N4 = 4.5

B1 = -15;
B2 = -13;
B3 = -14;
B4 = -1;

H = [1 0 0 0 ]'; % 0; 1 0 0 0 0; 1 0 0 0 0; 1 0 0 0 0];

F = [1]; % -1 -1 -1 -1; 0 1 0 0 0; 0 0 1 0 0; 0 0 0 1 0; 0 0 0 0 1];
Q = [5] % 0 0 0 0; 0 2 0 0 0; 0 0 2 0 0; 0 0 0 2 0; 0 0 0 0 2]
P = Q;

R = diag([N1, N2, N3, N4]);
x0 = [75]';% B1; B2; B3; B4]

for i = 1:1:100
    T1(i) = 70+N1*randn;%+B1
    T2(i) = 70+N2*randn;%+B2
    T3(i) = 70+N3*randn;%+B3
    T4(i) = 70+N4*randn;%+B4

    xobs(:,i) = [T1(i);T2(i);T3(i);T4(i)];

    x_est(:,i) = F*x0;

    P = F * P * F' + Q;
    K = P*H'*inv(H*P*H'+R);
    x(:,i) = x0 + K*(xobs(:,i)- H*x0);
    x0 = x(:,i);
end

plot(x(1,:));
hold on
plot(T1)
plot(T2)
plot(T3)
plot(T4)
grid
legend('x', 'T1', 'T2', 'T3', 'T4');
hold off
</code>```

答:
Your state vector $x$ is correct.

When you do bias estimation(look up IMU kalman filters) you assume that the bias is constant/slowly varying. So your $F$ matrix is just the identity(5x5).

Your $H$ matrix converts the predicted state into the form of the measurement. So essentially convert this equation into matrix form $T_{est}−B_{x}$ which is just $[1,−1,0,0,0]$ where the -1 will move depending on the sensor.

Also what will probably end up happening is that your sensor with the lowest error/covariance will be taken as the true value, and its bias will tend towards 0.


问:
Vehicle Odometry Correction Using Lidar Contour Points (Localization)
I am currently working on a project where...

Vehicle travels from point A to B

GPS trajectory is used as a reference

Vehicle odometry (bicycle model without slip angle beta) was used to estimate a trajectory, which resulted in odometry error

I am now given sets of lidar contour points & lane data (timestamps aligned with those of gps & odometry data), which I would have to use to correct the error and prove it

Note: The vehicle trajectory data I am using is NOT closed-loop.

My current thought is to use these lidar contour points to obtain rotational/translational vector between each frame and reverse-update the values to the vehicle to update the trajectory. When lacking lidar data, I'd like to use camera lane data to supplement the information.

I have read many articles on ICP and pose SLAM, yet there are some characteristics about my data that seem to hinder my progress.

ICP requires 'reference' lidar/map points which I do not have. Can I use points at t = x as a reference to update the points at t = x + 1?

I do not get same amount of lidar contour points per frame. That is, my data isn't in a PCL format. For example, some frames have 30 contour points whereas some frames have 150. (Some frames lack contour points). It seems that ICP algorithms require size of vectors to be the same. What would be the clever way to approach the problem when the number of contour points is fluctuating?

The lane data are given in coefficients constituting 3rd degree polynomial with start/end range for your information.

I have currently ONLY applied ICP to calculate R&T when the number of contour points at time t & t+1 is equal, and came up with R matrix of size 360x360. How would I apply this to vehicle that is located at a single point?

I do not necessarily have to use ICP to solve the problem, yet being new to the topic, I am kind of stuck at the moment. I'd appreciate some opinion/feedback. Thank you!

答:
ICP requires 'reference' lidar/map points which I do not have. Can I
use points at t = x as a reference to update the points at t = x + 1?

Reference just refers to a reference frame. Which can be just the previous frame, or one obtained earlier. So yes you can match t=x and t=x+1. That is how LIDAR odometry works.

I do not get same amount of lidar contour points per frame. That is,
my data isn't in a PCL format. For example, some frames have 30
contour points whereas some frames have 150. (Some frames lack contour
points). It seems that ICP algorithms require size of vectors to be
the same. What would be the clever way to approach the problem when
the number of contour points is fluctuating?

This is not true. The whole point of ICP (Iterative closest Point) is that it doesn't require explicit matches. Two points are considered a match if they are the closest pair to each other.

I have currently ONLY applied ICP to calculate R&T when the number of
contour points at time t & t+1 is equal, and came up with R matrix of
size 360x360. How would I apply this to vehicle that is located at a
single point?

You are doing something wrong. ICP should give you 1 $Rt$ which is composed of a 3x3 rotation matrix and 3x1 translation matrix.

The lane data are given in coefficients constituting 3rd degree
polynomial with start/end range for your information.

I would probably ignore these. Or just convert them to points using the start and end of the line. You could do something like ICL(Iterative Closest Line), but it could make the implementation a lot more complicated.


问:
Occupancy grid maps
Why do we take probabilistic approch to model the environment for robots in occupancy grid maps.The assumption is always taken that each cell on the grid map is either free or occupied.So why have probabilitic approch?</p>

答: As the wikipedia page of Occupancy grid mapping explains, the result of the mapping process is a binary 1 or 0, occupied or not, the decision itself may be based on noisy data, which involves the probabilistic assessment of prior information to infer the posterior probability of the occupancy.</p>

Title: Occupancy grid maps


问:
ICP(Iterative Closest Point) with Partially Overlapping Conditions & Changing Point Numbers
I am currently working on fixing vehicle odometry data using lidar contour points. Since I am receiving lidar data in the form of countour points, I thought I'd use ICP to correct the odometry error calculated from bicycle model.

Here is my question.

The code I am referencing from https://github.com/kissb2/PyICP-SLAM/blob/master/utils/ICP.py requires that number of points at time t = x must equal number of points at time t = x+k, or at least be in the array of same size. (SHOWN by assert statement "assert A.shape == B.shape" ) However, I do get fluctuating number of contour points per every time frame, thus this algorithm is not applicable to my problem.

Has anyone ever came across an ICP with differing array size of 'source frame' and 'target frame'? If so, how should I address the issue? If you just append the points into the arrays A & B of same size M with empty spots being zeros, would the ICP algorithm autonomously sort itself out despite receiving different number of data points? (For example, if A had 30 points and B had 60 points, append A with empty matrix of size 30 to make it into size 60.)

Thank you!

```<code>def icp(A, B, init_pose=None, max_iterations=20, tolerance=0.001):
    '''
    The Iterative Closest Point method: finds best-fit transform that maps points A on to points B
    Input:
        A: Nxm numpy array of source mD points
        B: Nxm numpy array of destination mD point
        init_pose: (m+1)x(m+1) homogeneous transformation
        max_iterations: exit algorithm after max_iterations
        tolerance: convergence criteria
    Output:
        T: final homogeneous transformation that maps A on to B
        distances: Euclidean distances (errors) of the nearest neighbor
        i: number of iterations to converge
    '''

    assert A.shape == B.shape

    # get number of dimensions
    m = A.shape[1]

    # make points homogeneous, copy them to maintain the originals
    src = np.ones((m+1,A.shape[0]))
    dst = np.ones((m+1,B.shape[0]))
    src[:m,:] = np.copy(A.T)
    dst[:m,:] = np.copy(B.T)

    # apply the initial pose estimation
    if init_pose is not None:
        src = np.dot(init_pose, src)

    prev_error = 0

    for i in range(max_iterations):
        # find the nearest neighbors between the current source and destination points
        distances, indices = nearest_neighbor(src[:m,:].T, dst[:m,:].T)

        # compute the transformation between the current source and nearest destination points
        T,_,_ = best_fit_transform(src[:m,:].T, dst[:m,indices].T)

        # update the current source
        src = np.dot(T, src)

        # check error
        mean_error = np.mean(distances)
        if np.abs(prev_error - mean_error) < tolerance:
            break
        prev_error = mean_error

    # calculate final transformation
    T,_,_ = best_fit_transform(A, src[:m,:].T)

    return T, distances, i
<span class="math-container">```
```

答:
ICP does not require that the number of points match. It can automatically take care of different sized inputs due to using the closest pair. An example of this can be found here, but in 2D.

In the PyICP-SLAM example you give you should notice that they randomly downsample the pointcloud to a fixed number(default 5000). That is how they are able to assert that the pointclouds are the same size. They mostly do this to increase the speed, but it can also give some better convergence properties.

So it is up to you to choose which method you want to implement. Randomly sample the point sets to being the same size, or let the algorithm run with different sized inputs.


问:
Given this old pin diagram and numbers on this old encoder, what would be a suitable modern encoder to use?
My place of work has recently acquired a Hydraulic Press Break from the 80's. It has a CNC controlled back stop which is driven by a DC motor and Screw drive. The encoder which couples to the motor provides feedback for the control loop.

I have determined the Encoder is faulty as when i rotate it clockwise by one turn it increments by 5mm but when I rotate it anti-clockwise by 1 turn it increments by 10mm (approximately). It also feels quite clunky when i rotate it.

I would like to replace the encoder however I am not sure how to go about choosing an encoder.

What do pins B and C on the pin diagram represent?
What do the numbers on the encoder represent?
Would this encoder have a zero pulse?

Encoder reads 32/500N05.47

答:
First, you need to find out how the encoder is used in your system. There are two sketches of plugs, I assume your encoder is connected as shown in the upper sketch.

Since the company who produced this is still producing and selling encoders, website here, I would start by giving them a call.

First of all ask, if they can provide a drop in replacement. It is a long shot, but not that uncommon in the automation/CNC world.

You can ask the manufacturer for a technical specification for the encoder. How many counts/revolution does the encoder output is the main important characteristic to choose a replacement. Voltage of the output signals is probably 5 V but it does not hurt to ask to make sure. It is unclear what "H-I = Motor" means.

The technical spec can help you shop around from other vendors. You can all sales people of different companies and ask them if they have something compatible to your spec sheet.


答:
Cause of the intrinsic noise in sensory data, we have to consider a probabilistic model (mostly Gaussian) for the sensor measurements. As a matter of fact, the description and definition of the mapping problem will be probabilistic. The goal is to compute the most likely map given the sensor data and commands given to the robot:

In occupancy grid mapping as the most simple method, each cell (grid) is a binary random variable:

Title: Occupancy grid maps

Body: Why do we take probabilistic approch to model the environment for robots in occupancy grid maps.The assumption is always taken that each cell on the grid map is either free or occupied.So why have probabilitic approch?</p>


问:
How do I get competent in using c++ for my projects?
I am a PhD student, working on Sensor Fusion and estimation problems. I would like as I finish my PhD to have acquired sound knowledge in what seems to be the industry norm for working with real perception systems (irregardless of context) , modern C++. This is a required skill in literally any job post that I see. I do have basic knowledge and experience of how it works, but it just feels simpler, more time efficient, easier to visualise, and easier to debug for a researcher to test an algorithm on Python or Matlab. I totally understand the benefits of using a scripting language, but I feel that we as engineers are over-relying on such tools.

Say for instance you want to test a RANSAC algorithm. My go-to approach would be start writing lines in MATLAB, no compilation necessary, all the tools are available in a single-line call, immediate access to documentation, very informative error messages, customisable plots on-the-fly and the list grows on. At the same time, I don't even want to imagine how much time it would take me to achieve the same results in C++, while most of the time I would have to spent it fighting with segmentation errors. So this brings me back to my question: How do I, in the context of my everyday PhD life, systematically build sound software development experience in C++, within my domain of expertise?

答:
Robot Operating System is a great framework for robotics, there are lots of libraries for SLAM and Sensor Fusion in general.

You can program in python and c++. My ideia is to take the advantage of the libraries written in c++. What i do (doesn't work for anyone) is just erase code inside some functions and implement my own function and test within the library.

C++ is essential, because thats the language you typically use to implement the algorithms on the actual robot.


问:
Kalman filter with missing dimension on measurement input
I am exploring the option of using a EKF with my differential drive robot. I do not have any prior experience with kalman filters.

The robot that is under consideration has two wheel encoders for odometry input using which can be used to calculate the displacement and heading change in the odom frame. It also has two magnetic sensors placed under either ends of the robot which will provide offset distance while traveling on top of a magnetic line.

Using the odometry model I can calculate the x, y and yaw of the bot over a period of time. And the magnetic measurement provides the y and yaw in world frame. Using this paper as my reference, my state transition matrix is jacobian of the odometry model as included below.

$$A = \begin{bmatrix}1 & 0 & -\Delta \sin(\theta_k + \omega/2)\\0 & 1 & \Delta \cos(\theta_k + \omega/2)\\0 & 0 & 1 \end{bmatrix}$$

So, the system model is

$$X_k = A * X_{k-1}$$

For initial values of $(0,0,0)$, when the robot is aligned with the world frame,

$$ \begin{bmatrix} x_{k+1} \\ y_{k+1} \\\theta_{k+1} \end{bmatrix} = \begin{bmatrix}1 & 0 & -\Delta \sin(\theta_k + \omega/2)\\0 & 1 & \Delta \cos(\theta_k + \omega/2)\\0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} 0 \\ 0 \\0 \end{bmatrix}$$

My measurement model will provide only y and yaw values. Hence, my H matrix and the z are as below

$$ H = \begin{bmatrix}0 & 1 & 0 \\ 0 & 0 & 1\\\end{bmatrix}$$
$$ z_k = \begin{bmatrix}y_k \\ \theta_k\end{bmatrix}$$

Going by the above model, if the robot is moving on top of the magnetic sensor in a straight line, my measured y and yaw will always be 0 and hence based on the equations outlined in the paper (also included above) my X value will never increment.

Any inputs to help me understand what I am missing here are highly appreciated.

[Edited]

The heading of the bot is along the x axis. So as it travels forward with 0 yaw the X will increase.

答: I can’t open the link to the paper you put in your question, but perhaps this is the way to think about the situation.  If your robot is facing along the y axis, and $\theta = 0$, then how can you get any motion in the x direction unless you first rotate $\theta$ until the wheels are not aligned with the y axis?   It seems to me the equation correctly reflects the physical constraints for such a nonholonomic system.


答:
I would say that in order to learn C++ up to an acceptable level there is no shortcut: you learn it by using it. And more often than not you learn it by using it together with others that know more than you.
Then, I would assess three things:

Are there C++ projects in my domain of expertise to which I’d like to contribute? This could be open-source ones, or some project colleagues in your lab always wanted to start.

How many hours per week do I want to invest on this? You still need to do your research, and it just won’t get easier learning another thing on top. Limiting it to an hour per day will keep things healthy for you.

There are plenty of free online resources for learning C++. Moreover, physical books are always helpful. Check these options.

More things:

Understand that this is simply going to take time. The amount depends on you.

Learn from the Masters! - check out videos of the best C++ conferences to learn the state of the art of things: Link to list of C++ conferences . Some conferences have “back to basics”-presentations which help beginners get engaged. Start with these ones before trying to tackle tougher ones.

Hope this helps.

Title: How do I get competent in using c++ for my projects?

Body:
I am a PhD student, working on Sensor Fusion and estimation problems. I would like as I finish my PhD to have acquired sound knowledge in what seems to be the industry norm for working with real perception systems (irregardless of context) , modern C++. This is a required skill in literally any job post that I see. I do have basic knowledge and experience of how it works, but it just feels simpler, more time efficient, easier to visualise, and easier to debug for a researcher to test an algorithm on Python or Matlab. I totally understand the benefits of using a scripting language, but I feel that we as engineers are over-relying on such tools.

Say for instance you want to test a RANSAC algorithm. My go-to approach would be start writing lines in MATLAB, no compilation necessary, all the tools are available in a single-line call, immediate access to documentation, very informative error messages, customisable plots on-the-fly and the list grows on. At the same time, I don't even want to imagine how much time it would take me to achieve the same results in C++, while most of the time I would have to spent it fighting with segmentation errors. So this brings me back to my question: How do I, in the context of my everyday PhD life, systematically build sound software development experience in C++, within my domain of expertise?


答:
If you want to incoorporate our sensors properly you first need to build a sensor model. This model tells you whenever your sensor measured something, what is the probability that this is the real value. You will get a distribution.

So when you measure 1m there is a prob. of .5 that thus measurement is true and a prob of .3 that the real distance ist actually 1.2m. To process this information you will not just modify one cell in you map but all adjacent cells. You need to to merge the distribution into the map.

HP Moravec - Sensor devices and systems for robotics, 1989 - Springer (original certainty map paper)

Probabilistic Sensor Models (lecture on this topic)

Title: Occupancy grid maps


问:
Will connecting two servo motors double the torque?
For my robot, I am using two continuous rotation servos to spin a threaded rod. I am trying to make this project as cheap as possible. Here are the servos that I can find:

Servo #1: This is a very cheap option and it has half of the torque I need.

Servo #2: This has all of the torque my project requires, but it is much more expensive that two of servo #1.

Can I hook up two of servo #1 to each end of the rod and have them move synchronized? I can spare a few extra pins on my microprocessor that I am using; that isn't a issue. I know hooking two together will increase torque, but I don't want 75% of the torque I want in this situation. Also, I don't care if I only have 98% of my torque "goal" with the extra weight (which probably won't happen) but I don't want to, like I said earlier, have 70, 80, 90% of my "target goal" of torque if possible.

Any help appreciated. Thanks in advance.

答: This guy put a spring in between his 2nd servo to protect them from any slight differences... Maybe this is what you're looking for? http://youtu.be/jqsmai2Nafk?t=1m13s</p>


问:
HTTP requestes for Robot Teleoperation
I'm new to this forum so I apologise if I disrespect some rules. I am considering using HTTP requests to control a robot remotely. The design problem I am trying to solve is that I would like to operate my robot using a wireless controller. The controller communicates to my pc, which then transmits data about its joystick to the robot. I am thinking of sending this data by means of HTTP requests. Based on the design problem, the communication will happen frequently.

Are there any glaring issues with this approach?

Are there any alternative communication protocols that might be better?

Thanks!

答:
If you want communicate over internet:

I think you should explore websockets for the purpose. Websocket connections allow both clients and server to send data. It's a full duplex communication mode.
If you are using python, you can simply use a websocket client package like this one and get the python code to talk to your server (and let server talk to the python code).

On user browser side too(assuming you want to control your robot using a web application), implementing websocket is fairly easy. If you are not using a web application for the user, you can use the same python websocket client method to get the job done.
Websocket server can be easily implemented in various langugages, however I suggest you not to use PHP for the job. It will make your life complicated. Check this link for example of a websocket server using Node.js

For a local network:

Go for ROS and do Network setup and get the job done fairly easily than working on RPC yourself.

Title: HTTP requestes for Robot Teleoperation


答:
The answer from @edwinem is

"You are confusing the jacobian matrix with the state transition equation. In the paper you referenced Eq 1 is what you use to update your state. It is also not even a matrix since it is non linear. The A matrix you have is used to update the covariance.You can see that the prediction step consists of 2 parts in Eq 9 and 10. One is used to update your state via some function (Eq 9,1), and the other is the jacobian of that function used to update your uncertainty(A matrix, Eq 10)"

Title: Kalman filter with missing dimension on measurement input

Body:
I am exploring the option of using a EKF with my differential drive robot. I do not have any prior experience with kalman filters.

The robot that is under consideration has two wheel encoders for odometry input using which can be used to calculate the displacement and heading change in the odom frame. It also has two magnetic sensors placed under either ends of the robot which will provide offset distance while traveling on top of a magnetic line.

Using the odometry model I can calculate the x, y and yaw of the bot over a period of time. And the magnetic measurement provides the y and yaw in world frame. Using this paper as my reference, my state transition matrix is jacobian of the odometry model as included below.

$$A = \begin{bmatrix}1 & 0 & -\Delta \sin(\theta_k + \omega/2)\\0 & 1 & \Delta \cos(\theta_k + \omega/2)\\0 & 0 & 1 \end{bmatrix}$$

So, the system model is

$$X_k = A * X_{k-1}$$

For initial values of $(0,0,0)$, when the robot is aligned with the world frame,

$$ \begin{bmatrix} x_{k+1} \\ y_{k+1} \\\theta_{k+1} \end{bmatrix} = \begin{bmatrix}1 & 0 & -\Delta \sin(\theta_k + \omega/2)\\0 & 1 & \Delta \cos(\theta_k + \omega/2)\\0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} 0 \\ 0 \\0 \end{bmatrix}$$

My measurement model will provide only y and yaw values. Hence, my H matrix and the z are as below

$$ H = \begin{bmatrix}0 & 1 & 0 \\ 0 & 0 & 1\\\end{bmatrix}$$
$$ z_k = \begin{bmatrix}y_k \\ \theta_k\end{bmatrix}$$

Going by the above model, if the robot is moving on top of the magnetic sensor in a straight line, my measured y and yaw will always be 0 and hence based on the equations outlined in the paper (also included above) my X value will never increment.

Any inputs to help me understand what I am missing here are highly appreciated.

[Edited]

The heading of the bot is along the x axis. So as it travels forward with 0 yaw the X will increase.


问:
Implementation of a particle filter algorithm
How do you calculate the likelihood of making an observation ? </p>

<p>Does someone have a link to a book or article that explains the math that you need to do to implement a PF algorithm?
Conceptually i understand the implementation but I'm having a hard time updating the particles after a measurement update. Any resource that goes over the math in details/ with examples would really help. </p>

答: "Probabilistic Robotics" by Sebastian Thrun, Wolfram Burgard, Dieter Fox is the book you are looking for.</p>


问:
ROS AMCL does not need odometry data?
I'm reading amcl document on ROS Wiki. In its subscribed topics there is not odometry topic, why? It works only with laser?

Subscribed Topics: (From ROS Wiki)

scan (sensor_msgs/LaserScan)

tf (tf/tfMessage)

initialpose (geometry_msgs/PoseWithCovarianceStamped)

map (nav_msgs/OccupancyGrid)

And my next question is how can I use amcl in Gazebo simulator for turtlebot? Any tutorial available?

答:
amcl receives the integrated odometry information over the tf topic between base_link and odom and then computes the correction between the odom frame and the map frame as the odometry accumulates drift.

Frames are defined in REP 105

Title: ROS AMCL does not need odometry data?


问:
In SLAM loop closure, What does a sim3 matrix do?
I am confused between 3D rigid affine(with scale, without shear) transformation and Lie group's sim(3) matrix.

3D affine would be [sR|t; 0 1].
we rotate, scale and then translate a 3D point using 3D affine transform

\begin{matrix}
s\cdot r_{11} & s\cdot r_{12} & s\cdot r_{13} & t_1\\
s\cdot r_{21} & s\cdot r_{22} & s\cdot r_{23} & t_2\\
s\cdot r_{31} & s\cdot r_{32} & s\cdot r_{33} & t_3\\
0 & 0 & 0 & 1
\end{matrix}

sim3, according to [1] is [R|t; 0 1/s]

\begin{matrix}
r_{11} & r_{12} & r_{13} & t_1\\
r_{21} & r_{22} & r_{23} & t_2\\
r_{31} & r_{32} & r_{33} & t_3\\
0 & 0 & 0 & s^{-1}
\end{matrix}

In loop closure context, for a candiate loop closure keyframe, we have  an old pose(SE3) and its calculated sim3. sim3 has a scale, rotation and translation part.

what is this sim3 actually? How do I get the new pose (SE3) using sim3 ?

How do I get the new set of 3D points visible on the old keyframe pose?

In ORB slam code, the authors find the new set of 3D points from old set by first finding camera coordinates of the 3D points (using old pose) and then multiplying it by the inverse of calculated sim3

If sim3 is a matrix that maps the old points to new points, why don't we multiply the 3D points directly with sim3 matrix?

Reference:
[1] http://ethaneade.com/lie_groups.pdf

答:
I think you'll find section IV. Loop Closure useful here:
http://www.roboticsproceedings.org/rss06/p10.pdf</p>