A robot arm is a chain of rigid segments. When the upper arm turns, the hand attached to it has to move with it, and the hand can still turn on its own. This page builds the simplest such chain: one arm segment, drawn in blue, with a hand, drawn in green, at its end. Everything is done with the 4 x 4 homogeneous matrices of the single cube examples. The only new idea is the order in which the matrices are multiplied.
- What does the two segment result show ?
- How are the two transformations chained ?
- Matlab Code
- How does the step by step program build the result ?
What does the two segment result show ?
Let's look at the final picture before the matrices, so that you know what the code is aiming for. The arm is tilted upward, and the hand sits at its far end with its own extra twist. The point to check is that the hand stays attached to the arm in every view.
The result of this example is as shown below.
Figure 1. The arm segment, in blue, and the hand, in green, after the transformation. The hand follows the tilt of the arm and adds its own 45 deg twist around the arm axis.
- The arm is a 3 x 1 x 1 box from x = -0.5 to x = 2.5. The hand is a C-shaped gripper with two fingers, which opens toward +x.
- In the x-z view, the arm rises to the right. It is turned by 30 deg around the y axis, so its far end at x = 2.5 moves to about (2.17, 0, 1.25).
- In the same view, the hand sits on the upper end of the arm. Its fingers point along the tilted arm axis, not along the original x axis.
- In the y-z view, the arm points toward the viewer and appears as a short box. The hand shows its 45 deg twist, which is a rotation around the arm axis.
- In the x-y view, the arm appears along the x axis again, because a rotation around y does not change the y coordinate. The hand appears at the right end, and the twist makes its blocks overlap in this view.
The hand moves with the arm : the tilt of the arm is applied to the hand as well, so the two parts stay connected.The hand also has its own motion : the 45 deg twist acts only on the hand and leaves the arm unchanged.Each flat view checks a different part : the x-z view shows the arm tilt, and the y-z view shows the hand twist.
How are the two transformations chained ?
The arm and the hand need different matrices, but the hand matrix must contain the arm matrix. Otherwise the hand would not move when the arm moves. So the question is in which order to multiply the pieces, and the rule turns out to be the same for any number of segments.
The arm uses one rotation, Rarm, which is the y rotation by π/6 (30 deg) in the red code. Its x and z angles are 0. The hand is defined around its own origin, like the arm. It needs three steps, applied from right to left:
vertex of the hand = Rarm x T(L1, 0, 0) x Rhand x vertex in the hand frame Rhand : twist the hand by π/4 (45 deg) around its own x axis T(L1, 0, 0) : move the hand origin to the joint at x = L1 = 2 on the arm Rarm : turn the arm and the attached hand together
The hand is twisted first, while it still sits at its own origin. That way the twist turns the hand around its own axis. It is then moved to the joint, and the arm rotation acts last on the arm and the hand together. With the angles of the example, the three factors multiply into one matrix for the hand.
Rarm T Rhand = [ 0.8660 -0.3536 -0.3536 1.7321 ]
[ 0 0.7071 -0.7071 0 ]
[ 0.5000 0.6124 0.6124 1.0000 ]
[ 0 0 0 1 ]
The last column is where the hand origin ends up. It is the joint (2, 0, 0) turned by 30 deg around y, which gives (1.732, 0, 1.0). The arm matrix alone maps the same joint to the same point, so the hand stays attached. This is exactly the check you can make in Figure 1.
The pattern extends to longer chains. A third segment, such as a finger on the hand, gets Rarm T1 Rhand T2 Rfinger. So each segment inherits the full product of its parent and adds its own offset and rotation on the right. Robotics calls this forward kinematics.
A child segment inherits the matrix of its parent : the hand matrix starts with Rarm, so the hand follows every motion of the arm.Local motion goes on the right : the hand twist is the rightmost factor, so it acts while the hand is still at its own origin.The translation places the joint : T(L1, 0, 0) moves the hand origin to the point of the arm where the hand is attached.The last column shows where the joint went : (1.732, 0, 1.0) here, the same point the arm matrix gives for the joint.
Matlab Code
Following is the MatLab code for this example. For now, don't pay too much about the code itself, just focus on number marked in red and try to undertand the mathematical meaning intuitively. You can just copy the code here into your Matlab and change the numbers in red part until you develop the intuitive understanding.
Note : When copy and paste this code, '...' may cause some error. In that case, erase '...' and retry '...' in your matlab editor.
clear all;
AxisRange = 4;
L1 = 2;
vertSeg1 = [-0.5 -0.5 -0.5; ...
-0.5 0.5 -0.5; ...
(0.5 + L1) 0.5 -0.5; ...
(0.5 + L1) -0.5 -0.5; ...
-0.5 -0.5 0.5; ...
-0.5 0.5 0.5; ...
(0.5 + L1) 0.5 0.5; ...
(0.5 + L1) -0.5 0.5];
facSeg1 = [1 2 3 4; ...
2 6 7 3; ...
4 3 7 8; ...
1 5 8 4; ...
1 2 6 5; ...
5 6 7 8];
vertHand1 = [0.00 0.5 -0.5; ...
0.5 0.5 -0.5; ...
0.5 1.0 -0.5; ...
1.5 1.0 -0.5; ...
1.5 0.5 -0.5; ...
1.0 0.5 -0.5; ...
1.0 -0.5 -0.5; ...
1.5 -0.5 -0.5; ...
1.5 -1.0 -0.5; ...
0.5 -1.0 -0.5; ...
0.5 -0.5 -0.5; ...
0.00 -0.5 -0.5; ...
0.00 0.5 0.5; ...
0.5 0.5 0.5; ...
0.5 1.0 0.5; ...
1.5 1.0 0.5; ...
1.5 0.5 0.5; ...
1.0 0.5 0.5; ...
1.0 -0.5 0.5; ...
1.5 -0.5 0.5; ...
1.5 -1.0 0.5; ...
0.5 -1.0 0.5; ...
0.5 -0.5 0.5; ...
0.00 -0.5 0.5; ...
];
facHand1 = [1 2 11 12; ...
2 3 4 5; ...
2 6 7 11; ...
11 8 9 10; ...
13 14 23 24; ...
14 15 16 17; ...
14 18 19 23; ...
23 20 21 22; ...
1 12 24 13; ...
1 13 14 2; ...
2 14 15 3; ...
3 15 16 4; ...
4 16 17 5; ...
5 17 18 6; ...
6 18 19 7; ...
7 19 20 8; ...
8 20 21 9; ...
9 21 22 10; ...
10 22 23 11; ...
11 23 24 12; ...
];
phiXSeg1 = 0;
TxMatrixXSeg1 = [1 0 0 0;...
0 cos(phiXSeg1) -sin(phiXSeg1) 0; ...
0 sin(phiXSeg1) cos(phiXSeg1) 0; ...
0 0 0 1];
phiYSeg1 = pi/6;
TxMatrixYSeg1 = [cos(phiYSeg1) 0 -sin(phiYSeg1) 0;...
0 1 0 0; ...
sin(phiYSeg1) 0 cos(phiYSeg1) 0; ...
0 0 0 1];
phiZSeg1 = 0;
TxMatrixZSeg1 = [cos(phiZSeg1) -sin(phiZSeg1) 0 0;...
sin(phiZSeg1) cos(phiZSeg1) 0 0; ...
0 0 1 0; ...
0 0 0 1];
TxMatrixShiftScaleHand1 = [1 0 0 L1;...
0 1 0 0; ...
0 0 1 0; ...
0 0 0 1];
phiXHand1 = pi/4;
TxMatrixXHand1 = [1 0 0 0;...
0 cos(phiXHand1) -sin(phiXHand1) 0; ...
0 sin(phiXHand1) cos(phiXHand1) 0; ...
0 0 0 1];
phiYHand1 = 0;
TxMatrixYHand1 = [cos(phiYHand1) 0 -sin(phiYHand1) 0;...
0 1 0 0; ...
sin(phiYHand1) 0 cos(phiYHand1) 0; ...
0 0 0 1];
phiZHand1 = 0;
TxMatrixZHand1 = [cos(phiZHand1) -sin(phiZHand1) 0 0;...
sin(phiZHand1) cos(phiZHand1) 0 0; ...
0 0 1 0; ...
0 0 0 1];
% transformed object
VerticesHomSeg1 = zeros(8,4);
for i = 1:8
VerticesHomSeg1(i,:) = [vertSeg1(i,:) 1];
end
TxMatrixRotSeg1 = TxMatrixZSeg1 * TxMatrixYSeg1 * TxMatrixXSeg1;
TxVerticesHomSeg1 = TxMatrixRotSeg1 * VerticesHomSeg1';
TxVerticesHomSeg1 = TxVerticesHomSeg1';
TxVerticesSeg1 = zeros(8,3);
for i = 1:8
TxVerticesSeg1(i,:) = TxVerticesHomSeg1(i,1:3);
end
% transforming object
VerticesHomHand1 = zeros(24,4);
for i = 1:24
VerticesHomHand1(i,:) = [vertHand1(i,:) 1];
end
TxMatrixRotSeg1 = TxMatrixZSeg1 * TxMatrixYSeg1 * TxMatrixXSeg1;
TxMatrixRotHand1 = TxMatrixXHand1 * TxMatrixYHand1 * TxMatrixZHand1;
TxVerticesHomHand1 = TxMatrixRotSeg1 ...
* TxMatrixShiftScaleHand1 ...
* TxMatrixRotHand1 ...
* VerticesHomHand1';
TxVerticesHomHand1 = TxVerticesHomHand1';
TxVerticesHand1 = zeros(24,3);
for i = 1:24
TxVerticesHand1(i,:) = TxVerticesHomHand1(i,1:3);
end
% plotting transformed arm and hand ---------------------------------------
subplot(2,2,1);
patch('Faces',facSeg1,'Vertices',TxVerticesSeg1,'FaceColor','b');
patch('Faces',facHand1,'Vertices',TxVerticesHand1,'FaceColor','g'); % draw the red cube
axis([-AxisRange AxisRange -AxisRange AxisRange -AxisRange AxisRange]);
grid();
material shiny;
alpha('color');
alphamap('rampdown');
view(30,30);
title('transformed');
set(gca,'xticklabel',[]);set(gca,'yticklabel',[]); set(gca,'zticklabel',[]);
% view along y-axis (x-z plane)
subplot(2,2,2);
patch('Faces',facSeg1,'Vertices',TxVerticesSeg1,'FaceColor','b');
patch('Faces',facHand1,'Vertices',TxVerticesHand1,'FaceColor','g');
axis([-AxisRange AxisRange -AxisRange AxisRange -AxisRange AxisRange]);
grid();
material shiny;
alpha('color');
alphamap('rampdown');
view(0,0);
title('x-z plane');
set(gca,'xticklabel',[]);set(gca,'yticklabel',[]); set(gca,'zticklabel',[]);
% view along x-axis (y-z plane)
subplot(2,2,3);
patch('Faces',facSeg1,'Vertices',TxVerticesSeg1,'FaceColor','b');
patch('Faces',facHand1,'Vertices',TxVerticesHand1,'FaceColor','g');
axis([-AxisRange AxisRange -AxisRange AxisRange -AxisRange AxisRange]);
grid();
material shiny;
alpha('color');
alphamap('rampdown');
view(90,0);
title('y-z plane');
set(gca,'xticklabel',[]);set(gca,'yticklabel',[]); set(gca,'zticklabel',[]);
% view along z-axis (x-y plane)
subplot(2,2,4);
patch('Faces',facSeg1,'Vertices',TxVerticesSeg1,'FaceColor','b');
patch('Faces',facHand1,'Vertices',TxVerticesHand1,'FaceColor','g');
axis([-AxisRange AxisRange -AxisRange AxisRange -AxisRange AxisRange]);
grid();
material shiny;
alpha('color');
alphamap('rampdown');
view(0,90);
title('x-y plane');
set(gca,'xticklabel',[]);set(gca,'yticklabel',[]); set(gca,'zticklabel',[]);
The listing defines two shapes and transforms them separately. The arm box uses vertSeg1 and facSeg1, which hold its 8 corners and 6 faces. The gripper uses vertHand1 and facHand1, which hold its 24 corners and 20 faces, with 12 corners at z = -0.5 and 12 at z = 0.5. The red lines define the angles and the matrices, and the blue lines combine them.
The blue lines are the heart of the example. The arm gets TxMatrixRotSeg1 only. The hand gets TxMatrixRotSeg1 * TxMatrixShiftScaleHand1 * TxMatrixRotHand1, which is exactly the chain above. Its translation matrix holds L1 in the last column of the first row, so it moves the hand to the joint at x = 2.
A few details of the listing can be confusing, although none of them changes the plot.
- The arm rotation is TxMatrixZSeg1 * TxMatrixYSeg1 * TxMatrixXSeg1, while the hand rotation is TxMatrixXHand1 * TxMatrixYHand1 * TxMatrixZHand1, in the reverse order. The results agree here only because each rotation has a single nonzero angle. With two nonzero hand angles, the two orders would give different hands.
- TxMatrixRotSeg1 is computed twice with the same value, once for the arm and once again before the hand. The second line is redundant.
- The name TxMatrixShiftScaleHand1 suggests a scaling, but the matrix only shifts. Its 3 x 3 block is the identity.
- The comment '% draw the red cube' sits on the line that draws the green hand. The comment '% transformed object' above the arm loop should read '% transforming object', like the one above the hand loop.
- The axis tick labels are removed with set(gca, ...), which is why Figure 1 shows grid lines without numbers. The axis range is +/-4 on every axis.
Two shapes, two matrices : the arm and the hand are transformed by separate products, and the hand product contains the arm product.L1 appears twice : it sets the length of the arm box and the shift of the hand, so changing it keeps the hand at the end of the arm.Keep one rotation order for all segments : the listing mixes Z*Y*X and X*Y*Z, which only works while each segment turns around one axis.
How does the step by step program build the result ?
I created another program for the same transformation, but just to show the detailed steps of transformation for each segment step by step. Following is the result of the programe. You may need a large screen to get the detailed look. The source code may look too intimidating, but it is just long.. the mathematical logic is exactly same as above. It got long just because of too many plots. Get the source from here and try it.
Figure 2. The same transformation split into steps. Each segment is shown before and after its own transformation, and the combined result is shown last.
- The first two rows show the arm alone. On the left it is the original red box along x. On the right it is the blue box tilted by 30 deg, which is visible in the x-z view.
- The next two rows show the hand alone. The red original is the C shape at its own origin, and the x-y view shows the two fingers clearly. The green result has already gone through the twist, the shift and the arm rotation.
- The bottom part shows the arm and the hand together in a large 3D view and in the three flat views. This part is the same result as Figure 1.
The hand in the third and fourth rows moves far from its own origin between the red and the green state. That jump is the shift by L1 followed by the arm rotation. When the arm is added in the bottom part, the green hand lands exactly on the end of the blue arm, because both use the same Rarm.
Split a chain into segments to debug it : a wrong factor shows up in the plot of the segment that uses it.The combined plot adds no new math : it only draws the two transformed shapes on the same axes.