Moveit2 Industrial Robot Manipulator

https://moveit.ros.org/

1.Robotics

https://www.bilibili.com/video/BV1v4411H7ez

https://www.bilibili.com/video/av59243185

1.1.Theoretical Foundation

1.1.1.DOF (degree of freedom)

In simple terms, degrees of freedom (hereinafter collectively referred to as dof) refer to the basic motion modes of an object in space , and there are 6 in total. Any motion can be decomposed into these 6 basic motion modes, and these 6 basic motion modes can be divided into two categories: translation and rotation .

Displacement: translation along the X-axis, Y-axis, and Z-axis

Rotation: Roll (rotation about X), Pitch (rotation about Y), Yaw (rotation about Z)

1.2.Mathematical Foundations

1.2.1.Representation of pose (position and orientation)

If there is a rigid body (frame) in a space, how do we determine the pose of the rigid body in this space?

First, a world frame must be established, and then a body frame must be established on the rigid body.

1. Description of the location

To describe the position of a rigid body's center of mass (a point) in the world, a 3x1 vector can be used.

In this way, the three DOFs of translation are known.

2. Description of orientation

Let the world coordinate system be A, and the rigid body coordinate system be B.

The above matrix is called a rotation matrix, which is a 3\*3 orthogonal matrix. ABR describes A as the reference coordinate system and the orientation of B relative to A.

Each column vector represents the direction in which the corresponding coordinate axis of B points.

Each column vector is the direction cosine (Direct Cosines) of the corresponding coordinate axis of B relative to A.

Each element r ij of the rotation matrix represents the direction cosine between the j-th axis of B and the i-th axis of A.

If you really can't understand it, let's look at the following example first:

How is B's X-axis represented in A? As can be seen, B's X happens to be the negative half-axis of A's Z-axis, which is 0, 0, -1.

How is B's Y-axis represented in A? As can be seen, B's Y is exactly the positive semi-axis of A's Y-axis, which is 0, 1, 0.

How is B's Z-axis represented in A? As can be seen, B's Z-axis is exactly the positive half-axis of A's X-axis, so it is 1, 0, 0.

In this example, the Z coordinates of A and B coincide, so we only need to look at the top view.

That is, just project the unit vector XB onto A's X and Y to see its components.

Similarly, YB is the same.

ZB and ZA coincide, which is relatively simple.

The answer is B.

3. Description of pose

Through the state of BF in WF, the pose of the rigid body in the world can be known.

4. Description of Motion

Red is the trajectory line of rigid body motion,

The derivative of the trajectory (translational DOF) with respect to time is the linear velocity of the rigid body.

The derivative of a rigid body's velocity with respect to time is the rigid body's linear acceleration.

Similarly, the derivative of the rotational DOF with respect to time is the angular velocity of the rigid body.

The derivative of angular velocity with respect to time is the angular acceleration of a rigid body.

1.2.2.Rotation Matrix

Features:

Since each element in the rotation matrix R is the inner product of two vectors, the inner product can have its operands swapped with the final resultremaining numerically unchanged.

So we choose to swap positions.

The original R is a matrix in which each column represents the components of one axis of B in the A frame.

After swapping positions, R is such that each row represents the components of one axis of A in the B frame.

Conclusion: Therefore, Rab = the transpose of Rba.

They are obviously in a transposed relationship.

RT\*R=I3 (3\*3 identity matrix) (property of orthogonal matrix)

Another property is that |R|=1.

Although there are 9 numbers, due to the properties of orthogonal matrices, there are constraints; among these 9 numbers, 6 numbers vary with changes in the other numbers, so in reality only 3 parameters among these 9 numbers can be chosen arbitrarily, which means the rotation matrix actually has only 3 degrees of freedom. (rotational DOF)

One function of the rotation matrix is as follows:

比如说另一个坐标系B相对于A坐标系绕X,Y,Z轴各自逆时针转动theta度的旋转矩阵。

在中国大陆教材中，常用R(X,theta)来代替图中的RXA(theta)。图中这个A指的是原坐标系，得出来的AP`也是原坐标下的坐标。

This way, by left-multiplying AP by R, we obtain the coordinates in frame A after rotating P. (Be sure to distinguish this clearly from the rotation coordinate transformation discussed below; they are very easy to confuse.)

After P rotates 30 degrees counterclockwise, its coordinates in theoriginal coordinate systemare 002.

Summary: The rotation matrix mainly has three uses, as shown in the figure below:

1.2.3.Coordinate transformation

1. Translation coordinate transformation

2. Rotation coordinate transformation

Here APX is a numerical value, XA and the like are vectors, the addition is vector addition, and what is finally obtained is the AP vector.

The important conclusion is AP = ABR*BP. (Note that it is matrix left multiplication.)

AP is the coordinate of P in frame A.

BP is the coordinates of P in the B-frame.

ABR is the rotation matrix of B relative to A, with A as the reference coordinate system.

1.2.4.Object transformation and inverse transformation

The order of an object's translations can be reversed, but the order of an object's rotations cannot be reversed, otherwise the orientation will be different.

There are mainly two disassembly methods: one is to set a fixed coordinate system and keep rotating according to this coordinate system,

Another approach is to assume the object's coordinate system.

1.3.Manipulator description method

Link 0 is generally also called base_Link

First, understand the relative relationship: Link i - 1 comes after Axis i-1 (of course, other descriptions also hold).

1.4.Describe the relationships between the joints

That is, the common perpendicular (unique solution), whose length is Link Length (link length).

Currently this only constrains the distance between the two axes; the two axes can still rotate, so the next parameter is needed: Link Twist.

This angle is formed by moving the latter axis along the common perpendicular toward the current axis; the resulting angle is called the Link Twist.

In other words, for any two rotation axes in space, we need two parameters for description, namely Link Length and Link Twist.

If there are multiple joints connected in a chain, we would not be able to find the corresponding relationship, for example

With the above figure, I cannot represent the relative relationship and relative pose between ai-1 and the AI on the axis Axis i.

Therefore, other parameters are also needed.

First, of course, a length is needed: the distance between the two common perpendiculars along Axis i, called the Link Offset.

Then another angle is also needed, the Joint Angle link angle, also called the joint angle.

In fact, it is found that among these four parameters, only one parameter varies, while the others are fixed.

如果ioint type是revolute joint，那么thetai变化，其他不变。

If the joint type is a prismatic joint, then di changes while the others remain unchanged.

Describing two axes requires 2 parameters: link length and link twist angle.

Describing multiple axes connected in series requires 4 parameters (every two links require 4 parameters): link length, link twist, link offset, and joint angle.

1.5.Establish a frame on the joint

We generally define the Z direction to be the same as the direction of the rotation axis; whether Z points up or down depends on the orientation, and the angle between these two axes is the smallest, thus allowing the Z direction to be determined.

Xi's direction is along the direction of AI.

Xi is perpendicular to both Zi+1 and Zi.

Right-hand rule, determine the Y direction.

The origin is the intersection of Z and X.

If base_link(link0) and link1 are established, then it is a special case. (base_link is immobile and does not move)

frame0 and frame1 overlap (coincide). Usually, for example, in the case of a revolute joint, although theta is specified as arbitrary, I fix theta to 0, and then let frame0 and frame1 (theta

\= 0). If it is a prismatic joint, then similarly take frame1 when d=0. Note that here it is overlapping coincidence, not identical in shape, but completely overlapping coordinate frames.

The last link is similar, because both Xn and Xn-1 must be perpendicular to Axis n (Zn), so the simplest approach is to align Xn with the Xn-1 direction.

Xn takes the direction of Xn-1. That is, framen and framen-1 are extended.

The following is the most important among the key points: (There are several methods to judge; if there are errors, please discuss and revise.)

Common knowledge to know:

1. Generally, the connecting rod torsion angle is positive when counterclockwise.

2. The steering direction we commonly refer to is viewed from the opposite direction, that is, viewed with the arrow pointing toward the eye.

3. Viewing the steering direction from the opposite direction of the shaft versus viewing it along the direction are completely opposite directions, but on a plane it is easy to produce a visual illusion, making it difficult to understand. You can take a pen or a motor and try rotating it.

①Whether alphai-1 is positive or negative depends on whether the angle from Zi-1 to Zi is clockwise or counterclockwise. Extend your right hand and point your thumb along the direction of Xi-1. If alphai-1 follows the direction of the four fingers, then it is counterclockwise and positive; otherwise, it is clockwise and negative.

So as shown in the figure, alphai-1 is counterclockwise, so it is positive.

② Since ai-1 is a length, it is always positive, and its value is the relative distance along the Z-axis.

③The angle of thetai is basically the same. Point the right thumb along the direction of Zi; if thetai follows the direction of the four fingers, it is counterclockwise; if it is opposite, it is clockwise.

④di的大小方向要看从ai-1沿着zi的方向到ai，则是正值，反之为负值，大小即为距离。

1.6.Link Transformations

1.6.1.Theory

Both of our joints have two axes, one is Axisi-1 and the other is Axisi. We also have two frames, one is framei-1 and the other is framei.

We need to find what the relationship between the two frames is, that is, find the transformation matrix.

Then quantize the Trans Matrix.

Suppose there is a point P, and its representation in frame i is Pi. If we find the Ti-1 matrix of i, then there is a way to obtain the representation of P in frame i-1.

So now we need to use the four parameters we just found to convert them into our Trans Matrix.

These four parameters, namely ai-1, alphai-1, di, and thetai, are sufficient to express framei-1 to framei.

①First, on the Axis i-1,

First describe alpha, then rotate the framei-1 of Zi-1 to approximately the Zi direction to generate FrameR (rotate only Z, keep X unchanged, then determine Y using the right-hand rule)

② Then describe a,

Just move FrameR along the direction of ai-1 onto Zi,

③ Describe theta again

We rotate ZR in frameR so that it aligns with the ai direction, generating frameP (X moves, Z does not move, and Y is determined by the right-hand rule).

After doing this, the direction of Xp is the same as Xi, and the direction of Zp is also the same as Zi.

④ Then describe d, that is, pull FrameP upward, and finally it will coincide with Framei.

That is, from Frame i - 1 to Frame R, then to Frame Q, then to Frame P, and finally to Frame i. There are four transformations in total.

What we just demonstrated was from Pi-1 to Pi. Now what we require is from our Pi to Pi-1, so then it is left-multiplying in reverse order: first multiply by Tp i, then continue downward in this way, and so on.

The homogeneous transformation matrix T between the coordinate systems from link i to link i-1 i-1 i=Rot(X, aplhai-1)Trans(ai-1,0,0)Rot(Z,thetai)Trans(0,0,di)

The upper left is a 3x3 rotation matrix, so there are only the angle theta and alpha parameters,

The 3X1 matrix in the upper right corner is the vector of the origin of frame i relative to the origin of frame i - 1. Then it is viewed from frame i - 1. So it is a combination of length and angle.

In the last row, the 1X4 matrix, 0001 is a fixed number that does not change.

For continuous members, we can compute all the way from base_link to linkx, where x can be any number you like.

For example, if there are now three links, we find T23 (3 relative to 2), T12 (1 relative to 2), and T01 (ground relative to 1), then we can find T03 (ground relative to 3).

1.6.2.Example

1.6.2.1.Planar RRR type

① First, locate the joint rotation axis Joint Axes

The three rotation axes are three red dots. If they are dots, then that means the direction out of the paper surface. (Because it is planar, the angle between each Z-axis is 0, so the Link Twist Alpha is 0. Therefore, the direction of Z can be chosen arbitrarily, but here we assume that all joints rotate counterclockwise. According to the right-hand screw rule, all Z axes point upward.)

② Then find the common perpendiculars

However, since the Axes are all parallel to each other, there are infinitely many common perpendicular lines, so we can simply represent it within one plane.

③ Next, define the Zi vector (the Z direction is the same as the rotation axis direction, so it is also upward.)

④ Then determine the Xi vector in the middle

⑤ Then determine the Yi vector in the middle

⑥ Then determine the head and tail, that is, frame 0 and frame n

Actually, we can make frame0 and frame1 coincide completely, that is, let both alpha and theta be 0; as shown in the figure, they do not coincide, so there is a value for theta.

The same applies to the last member; it is recommended to make the direction of X3 coincide with the direction of X2, in which case both theta and alpha are 0.

Since each axis is parallel, alpha is 0, and because the ai are all coplanar, d is also 0.

Because frame0 and frame1 coincide, a0 = 0, then a1 = L1, a2 = L2

Since they are all RRR, theta are all varying angles.

For the P point end effector, it can also be calculated: move along the X3 direction to obtain the expression of P in frame n, and then the expression of P in frame 0 can be derived.

As shown in the figure, the coordinates of point P in Frame3 are (L3, 0, 0)

1.6.2.2.RPR type

① First locate the rotating shaft

Use the right-hand rule.

② Find the common perpendicular

Because Z1 of frame1 and Z2 of frame2 intersect, there is no common perpendicular.

The Z2 of frame2 and the Z3 of frame3 coincide, so there is also no common perpendicular.

That is to say, all elements of a are 0.

③Establish Zi vector

The Zi direction is the same as the direction of the rotation axis.

④Xi's vector

When Z1 and Z2 intersect, we choose the direction of X to be perpendicular to both Z1 and Z2. There are two options: either X points forward or backward; as shown in the figure, it points backward.

X1 and X2 must be parallel because it is a P-type joint

⑤Y-axis

⑥Frame 0 and frame n

frame 0 and frame 1 coincide

Make the X3 direction the same as the X2 direction

As shown in the figure, the driven joint parameters are theta1, d2, theta3 respectively.

Since frame0 and frame1 coincide, alpha0 = 0,

The angle from Z1 to Z2: point the right-hand thumb in the X1 direction, then the four fingers are in the same direction as Z1 to Z2, so it is counterclockwise and positive, therefore it is 90 degrees.

Then from frame 2 to frame 3, Z is collinear, so alpha2=0

Then, since some of the Zi intersect and some are collinear, all a are 0.

Then d1 is 0, because frame0 and frame1 coincide,

d2 is d2, d3 is L2 (d is the distance of X on the Axis)

The coordinates of point P on Frame3 are (0, 0, L3).

Actually, there are two choices for the Z direction and also two choices for X, making four choices in total; just choose the option that is easier for you to understand and calculate.

1.6.2.3.TSMC wafer robot in China (PRRR type with 4 degrees of freedom)

1.6.2.4.SCARA robot (RRRP type, 4 degrees of freedom)

The robot's last joint can be either R or P, so it is an RP joint, and either R or P can be calculated first.

1.6.2.5.RP Type

Choose D, because there are two degrees of freedom, so there are two driving parameters.

1.7.Actuator Joint and Cartesian Spaces

Our driver here is theta1-3,

Once we know the value of theta1-3, we will know the representation of point P in the world coordinate system.

This is called forward kinematics.

Calculating the joint angles from the world coordinate system of point P is called Inverse Kinematics.

Actuator Space is the actuator space, for example, how a motor can be controlled to rotate to a fixed angle in joint space (through a series of transformations).

There are parts that rotate and parts that translate (so two motors are required to achieve these two degrees of freedom).

The timing belt mechanism causes it to rotate. (The first motor)

The rack-and-pinion mechanism achieves up-and-down movement. (Second motor)

However, these two are not independent, because they are coaxially driven, so there is some load.

Two rotations become one rotation and one translation.

1.8.Forward kinematics

定义 ：已知机器人各个关节（或轮子等驱动单元）的运动参数（如角度、位移、速度等），计算末端执行器的位置和姿态。

ads=dv/dt * ds = ds/dt *dv = vdv

Newton's second law, conservation of energy, impulse and momentum

Wp is the coordinates of point P in the world coordinate system

According to this formula, the coordinates of the end effector in the world coordinate system can be obtained.

1.9.Inverse Kinematics

定义 ：已知末端执行器的目标位置和姿态，计算需要让各个关节（或轮子等驱动单元）运动到什么角度或速度才能达到该目标。

First, know the expression of a certain point P of the end effector in the world coordinate system, that is, given Pw or the frameH at a certain point on the end effector,

Find theta from Pw.

This way, the arm has 6 unknowns

16 numbers, the rotating part occupies 9 numbers, that is, the 3X3 rotation matrix in the upper left corner, and then what does the 3X1 vector in the upper right corner represent as the displacement relative to the origin? (That is, what is the displacement of the origin of frame6 relative to the origin of frame0?)

The following 0001 is an integer, fixed, and unchanging.

In this rotation matrix, there are 3 length conditions and 3 mutual perpendicularity conditions, so among the 9 numbers, only 3 degrees of freedom remain. (That is, the vector length being 1 imposes 3 constraints, and the vectors being pairwise perpendicular imposes 3 constraints, so it is a translation matrix with 3 degrees of freedom.)

Then, in the 3X1 vector in the upper right corner, the coordinates X, Y, Z relative to the origin correspond to 3 degrees of freedom.

Therefore, there are a total of 6 degrees of freedom.

These 12 equations mean that except for the 0001 below, each parameter above can be expressed by an equation. What we need to do is solve for the 6 unknowns from the 12 equations.

The dexterous workspace is a subset of the reachable workspace.

Its reachable workspace is a circular ring.

For a certain point, in this example, only 1 or 2 poses can be reached. This robot has only RWS, not DWS.

If the arms were the same length, then the workspace would be a circle,

There is a point, which is the DWS, i.e., the origin. When the arm folds inward, it can reach this point at any angle in 360 degrees, so this point is the DWS.