MoveIt2 industrial robot arm
Robotics
https://www.bilibili.com/video/BV1v4411H7ez
https://www.bilibili.com/video/av59243185
Theoretical Foundation
DOF (Degrees of Freedom)
Simply put, degrees of freedom (hereafter referred to as DOF) refer to the basic ways an object can move in space, totaling six types. Any motion can be broken down into these six fundamental movements, which can be further divided into two categories: translation and rotation.
Displacement: translation along the X, Y, and Z axes
Rotation: Roll (rotation around X), Pitch (rotation around Y), Yaw (rotation around Z).
Mathematical Foundations
Pose (position and orientation) representation
If there is a rigid body (frame) in a space, how do we determine its pose in that space?
First, establish a world frame, then set up a body frame on the rigid body.
- Position Description
Describing the position of a rigid body's center of mass (a point) in the world can be represented using a 3x1 vector.
This tells us the three DOF of translation.
- Description of Orientation
Let the world coordinate system be A and the rigid body coordinate system be B.
The matrix above is called a Rotation Matrix. It is a 3x3 orthogonal matrix. ABR describes the orientation of B relative to A, with A as the reference coordinate system.
Each column vector represents the direction in which the corresponding coordinate axis of B points.
Each column vector is the direction cosine of the corresponding axis of B relative to A.
Each element r_ij of the rotation matrix represents the direction cosine between the j-th axis of frame B and the i-th axis of frame A.
I really can't understand it, so let's first look at the example below:
How is the X-axis of B represented in A? It can be seen that the X-axis of B is exactly the negative half of the Z-axis of A, which is 0, 0, -1.
How is B's Y-axis represented in A? It can be seen that B's Y-axis aligns exactly with the positive half of A's Y-axis, so it is 0, 1, 0.
How is B's Z-axis represented in A? It can be seen that B's Z-axis is exactly the positive half of A's X-axis, so it is 1, 0, 0.
In this example, the Z axes of A and B coincide, so we only need to look at the top view.
Simply project the unit vector XB onto the X and Y axes of A to obtain its components.
The same applies to YB.
ZB and ZA overlap, which is relatively simple.
The answer is B.
- Description of Pose
By using the BF state in the WF, the pose of the rigid body in the world can be determined.
- Description of Motion
The red line is the trajectory 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 rigid body velocity with respect to time is the rigid body linear acceleration.
Similarly, the derivative of the DOF rotation 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.
Rotation matrix
Features:
Since each element in the rotation matrix R is the inner product of two vectors, the inner product is commutative and the final result remains numerically unchanged.
So we choose to swap positions.
The original R has each column as the component of a certain axis of B in frame A.
After swapping positions, each row of R is the component of a certain axis of A in the B frame.
Conclusion: Therefore, Rab = Rba^T.
They are clearly in a transposed relationship.
RT*R = I3 (3x3 identity matrix) (property of an orthogonal matrix)
There is also a property that|R|=1
Although there are 9 numbers, due to the properties of an orthogonal matrix, there are constraints. Among these 9 numbers, 6 change as the others vary, so in reality only 3 parameters can be freely chosen. This means the rotation matrix actually has only 3 degrees of freedom (rotational DOF).
One function of the rotation matrix is as follows:
For example, the rotation matrix of another coordinate system B rotating counterclockwise by theta degrees around the X, Y, and Z axes relative to coordinate system A.
In mainland Chinese textbooks, R(X, theta) is commonly used in place of R_X^A(theta) shown in the figure. The A in the figure refers to the original coordinate system, and the resulting ^AP' is also the coordinate in the original coordinate system.
In this case, left-multiplying A by R gives the coordinates of P in frame A after rotation. (Be sure to distinguish this from the rotation coordinate transformation discussed below, as they are easily confused.)
After rotating P 30 degrees counterclockwise, its coordinates in the original coordinate system are 002.
Summary: The rotation matrix primarily has three uses, as shown in the figure below:
coordinate transformation
- Translation: Translation of coordinate axes
- Rotating coordinate transformation
Here, APX is a scalar value, XA and others are vectors, the addition is vector addition, and the result is the vector AP.
The key conclusion is that AP = ABR * BP. (Note that this is left multiplication of matrices.)
AP is the coordinate of P in the A system.
BP is the coordinate of P in the B frame.
ABR is the rotation matrix of B relative to A, where A is the reference coordinate system.
Object transformations and inverse transformations
The order of translational movements can be reversed, but the order of rotational movements cannot be reversed; otherwise, the orientation will be different.
There are two main disassembly methods: one is to set a fixed coordinate system and keep rotating according to that coordinate system.
Another way is to assume the object's coordinate system.
Ways to describe a robotic arm
Link 0 is also commonly called base_link.
First, understand the relative relationship: Axis i-1 is followed by Link i-1 (though other descriptions are also valid).
Describe the relationships between each joint.
That is the common perpendicular (unique solution), and its length is the Link Length.
Now only the distances of the two axes are constrained; the two axes can still rotate. Therefore, the next parameter needed is the Link Twist angle.
This angle is formed along the perpendicular bisector by moving the next axis toward the current axis along that perpendicular bisector, and the resulting angle is called the Link Twist.
In other words, for any two axes in space, we need two parameters to describe them: Link Length and Link Twist.
If there are multiple shafts connected in series, we will no longer be able to find the corresponding relationship. For example,
In the image above, I cannot represent the relative relationship and relative pose between ai-1 and ai on the Axis i.
So other parameters are still needed.
First, you need a length — the distance between two common perpendiculars along Axis i, called the Link Offset.
Then we also need an angle, the Joint Angle, also called the joint angle.
Actually, it turns out that among these four parameters, only one changes while the others remain fixed.
If the joint type is a revolute joint, then theta_i changes while everything else remains the same.
If the joint type is a prismatic joint, then di changes while the others remain unchanged.
Two axes require two parameters: link length and link twist angle.
Describing multiple axes connected in series requires four parameters (each pair of links requires four parameters): Link Length, Link Twist, Link Offset, and Joint Angle.
Establish a frame on the joint.
We generally define the Z direction to be the same as the axis of rotation. Whether Z points upward or downward depends on the orientation, and the angle between these two axes is minimized, which allows us to determine the direction of Z.
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 establishing base_link (link0) and link1, this is a special case. (base_link is immobile and does not move.)
frame0 and frame1 overlap (coincide). Typically, for example, with a revolute joint, although theta is defined as arbitrary, I fix theta to 0 and then let frame0 and frame1 (theta
= 0). For a prismatic joint, similarly, take frame 1 when d=0. Note that this refers to overlapping coincidence, not identical shape, but completely overlapping coordinate systems.
The last link is similar, because both Xn and Xn-1 need to be perpendicular to Axis n (Zn), so the simplest method is to make Xn align with the direction of Xn-1.
Xn takes the direction of Xn-1. That is, frame n and frame n-1 are extended.
Here's the most important part: (There are several ways to determine this; if there are any errors, please discuss and revise.)
Common knowledge you need to know:
- Generally, the link twist angle is positive when measured counterclockwise.
- The steering we often refer to is viewed from the opposite direction, meaning we look in the direction where the arrow points toward the eyes.
- Looking at the rotation from the reverse direction of the axis versus along the direction of the axis results in completely opposite turning directions. However, on a flat plane, this can easily cause visual illusions and be difficult to understand. You can try rotating a pen or a motor to test it out.
① 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 with your thumb pointing in the direction of Xi-1. If alphai-1 follows the direction of your curled fingers, it is counterclockwise and positive; otherwise, it is clockwise and negative.
So as shown in the figure, alphai-1 is counterclockwise, so it is a positive value.
② The length of ai-1 is always a positive value because it represents a length, and its value is the relative distance along the Z-axis.
③ The angle of theta_i is essentially the same: point your right thumb along the direction of Z_i. If theta_i follows the direction of your curled fingers, it is counterclockwise; if it goes against them, it is clockwise.
④ The magnitude and direction of di depend on the distance from ai-1 to ai along the zi direction: if moving in the positive zi direction, the value is positive; otherwise, it is negative. The magnitude is simply the distance.
Link Transformations
theory
Both of our joints have two axes: one is Axis i-1 and the other is Axis i. We also have two frames: one is Frame i-1 and the other is Frame i.
We need to find the relationship between the two frames, that is, to find the transformation matrix.
Then quantize the Trans Matrix.
Suppose there is a point P, whose representation in frame i is Pi. If we find the matrix Ti-1 i, then we can obtain the representation of P in frame i-1.
So now we need to convert the four parameters we just found into our Transformation Matrix.
These four parameters—ai-1, alphai-1, di, and thetai—are sufficient to describe the transformation from framei-1 to framei.
① First, on Axis i-1,
First, describe alpha by rotating Zi-1 of frame i-1 to roughly align with the direction of Zi, generating FrameR (rotate only Z, leave X unchanged, then determine Y using the right-hand rule).
② Then describe a,
Just move FrameR along the direction of a_i-1 onto Z_i.
③ Describe theta again
We rotate ZR in frameR to align it with the direction of ai, generating frameP (X moves, Z stays fixed, 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.
④ Describe d again, which is pulling FrameP upward until it eventually coincides with Framei.
That is, from Frame i-1 to Frame R, then to Frame Q, then to Frame P, and finally to Frame i. A total of four transformations.
Just now we demonstrated going from Pi-1 to Pi. Now we need to go from our Pi to Pi-1, which means multiplying on the left in reverse order: first multiply by Tp i, then continue down the line.
The homogeneous transformation matrix between the coordinate systems from link i to link i-1 is T i-1 i = Rot(X, α_i-1) Trans(a_i-1, 0, 0) Rot(Z, θ_i) Trans(0, 0, d_i).
The top-left corner is a 3x3 rotation matrix, so it only has the angle theta and alpha parameters.
The 3x1 matrix in the upper right corner is the vector from the origin of frame i relative to the origin of frame i - 1, as seen from frame i - 1. So it is a combination of length and angle.
The 1x4 matrix in the last row, which is 0001, is a fixed number and does not move.
For continuous links, we can compute all the way from base_link to linkx, where x can be any number you want.
For example, with three links, if we find T23 (3 relative to 2), T12 (1 relative to 2), and T01 (ground relative to 1), we can find T03 (ground relative to 3).
Example
Planar RRR type
① First, locate the joint axes.
The three joints are represented by three red dots. Since they are points, this indicates the direction coming out of the page. (Because it is a planar structure, the angle between each Z-axis is 0, so the Link Twist Alpha is 0. Therefore, the direction of Z can be chosen arbitrarily. However, here we assume all joints rotate counterclockwise. Following the right-hand rule, all Z-axes point upward.)
② Then find the common perpendiculars.
However, since all axes are parallel to each other, there are infinitely many common perpendiculars. Therefore, we can simply express them within a single 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 middle Xi vector
⑤ Then determine the middle Yi vector.
⑥ Then determine the head and tail, i.e., frame 0 and frame n.
Actually, we can make frame0 and frame1 completely coincident by setting both alpha and theta to 0. However, as shown in the figure, they are not coincident, so there is a non-zero theta value.
The last link is the same. It is recommended to align the direction of X3 with that of X2, so that both theta and alpha are 0.
Because each axis is parallel, alpha is 0. Since all ai are coplanar, d is also 0.
Since frame0 and frame1 coincide, a0 = 0, then a1 = L1, a2 = L2.
Since everything is RRR, all thetas are varying angles.
The end effector at point P can also be calculated by moving 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 Frame 3 are (L3, 0, 0).
RPR type
① First, find the rotating shaft.
Follow 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 no common perpendicular either.
So that means all a's are 0.
③ Establish the Zi vector
The Zi direction is the same as the axis direction.
④ 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: X can point forward or backward. 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 joint parameters of the drive are theta1, d2, and theta3 respectively.
Since frame0 and frame1 coincide, alpha0 = 0.
The angle from Z1 to Z2: extend your right thumb pointing toward X1, then align your fingers with the direction from Z1 to Z2. This indicates a counterclockwise rotation, which is positive, so the angle is 90 degrees.
Then from frame2 to frame3, Z is collinear, so alpha2 = 0.
Then, because some Zi intersect and some are collinear, all a are 0.
Then d1 is 0, because frame0 and frame1 coincide.
d2 is d2, and d3 is L2 (where 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 two for the X direction, making four options in total. Just pick the one that is easiest to understand and calculate.
China TSMC wafer robot (PRRR type, 4 degrees of freedom)
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 you can calculate R first or P first.
RP type
Choose D, because with two degrees of freedom, there are two driving parameters.
Actuator Joint and Cartesian Spaces
Our drivers here are theta1-3.
Once we know the values of theta1-3, we will know the expression of point P in the world coordinate system.
This is called Forward Kinematics.
Inverse kinematics refers to the process of calculating joint angles from the world coordinates of point P.
Actuator Space refers to the drive space, such as how a motor is controlled to rotate to a fixed angle in joint space (through a series of transformations).
There are parts that both rotate and move (so two motors are needed to achieve these two degrees of freedom).
The synchronous belt mechanism rotates it. (First motor)
The rack and pinion mechanism achieves vertical 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.
Forward kinematics
Definition: Given the motion parameters (such as angle, displacement, velocity, etc.) of each joint (or drive unit like wheels) of a robot, calculate the position and orientation of the end effector.
ads=dv/dt * ds = ds/dt *dv = vdv
Newton's second law, conservation of energy, impulse and momentum.
Wp is the coordinate 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.
Inverse kinematics
Definition: Given the target position and orientation of the end effector, calculate the angle or speed at which each joint (or drive unit such as a wheel) must move to achieve that target.
First, know the expression of a certain point P on the end effector in the world coordinate system, that is, provide Pw or the frame H on a certain point of the end effector.
Calculate theta from Pw.
This gives the arm 6 unknowns.
16 numbers, the rotating part occupies 9 numbers, which is the 3x3 rotation matrix in the upper left corner. Then the 3x1 vector in the upper right corner represents 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 constraints and 3 perpendicularity constraints, so out of the 9 numbers, only 3 degrees of freedom remain. (That is, 3 constraints from unit vector lengths, and 3 from pairwise perpendicularity, so it's a translation matrix with 3 degrees of freedom.)
Then, in the 3x1 vector at the top right, the coordinates X, Y, Z relative to the origin represent 3 degrees of freedom.
So there are 6 degrees of freedom in total.
These 12 equations allow us to write an expression for each parameter above, except for the lowly 0001. What we need to do is solve for 6 unknowns from these 12 equations.
A dexterous workspace is a subset of the reachable workspace.
Its reachable workspace is a ring.
For a given point, in this example, only 1 or 2 poses can be achieved. This robot only has RWS, not DWS.
If the arms are the same length, the workspace becomes a circle.
One key point is DWS, which is the origin point. When the arm is folded inward, it can reach this point from any angle of 360 degrees, so that point is DWS.