Forward Kinematics

1. Homogeneous Transform

1.1 Representing Points

The point (x,y,z) is represented as the vector $\begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}$ . The 1 is a weighting factor. So the vectors $\begin{bmatrix} 2x\\ 2y\\ 2z\\ 2\\ \end{bmatrix}$ and $\begin{bmatrix} 257x\\ 257y\\ 257z\\ 257\\ \end{bmatrix}$ all represent that same point (x, y, z). Most of the time we will simply use a weighting factor of 1. Consider the more concrete example depicted in Figure 1.

Figure 1. When the plane takes-off, its nose is still at location (0,0,0) in plane coordinates, but it is at a different location in tower coordinates. To determine where it is in tower coordinates, the tower’s x and y axes have been extended, and the nose of the plane has been plotted in the manner of Figure 10. Thus we see that the nose of the plane is at location (30, 15, 10) in tower coordinates.
The plane is at location (30, 15, 10) in tower coordinates. We will normally represent that location as $\begin{bmatrix} 30\\ 15\\ 10\\ 1\\ \end{bmatrix}$ , however it can also be represented as $\begin{bmatrix} 60\\ 30\\ 20\\ 2\\ \end{bmatrix}$ , $\begin{bmatrix} -30\\ -15\\ -10\\ -1\\ \end{bmatrix}$ , $\begin{bmatrix} 75\\ 37.5\\ 25\\ 2.5\\ \end{bmatrix}$ , and an infinite number of other vectors.
In order to save space in this document, we will often write the point $\begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}$ as $\begin{bmatrix} x\ y\ z\ 1\\ \end{bmatrix}^{T}$ (the transpose of $\begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}$ ).

1.2 The Use of Multiple Coordinate Frames in Robotics

It is very common in robotics to use two or more coordinate frames to solve a problem. Suppose the airplane in Figure 11 were automatically controlled. It would be very useful to keep track of some things in tower coordinates. For example, the altitude of the plane is simply the z coordinate of its location in tower coordinates. It also would be useful to keep track of other things in airplane coordinates. For example, the direction the plane should head to in order to avoid a mountain. Indeed, for many mobile robot applications, it is desirable to know the locations of objects in both “world coordinates” and “robot coordinates.”

Multiple coordinate frames are also useful in traditional static robots. For example, consider the simple robot arm depicted in Figure 2. If we want to have the gripper pick up a widget off of a table, then we need to figure out the widget’s location. Perhaps we have a camera that we use to initially determine the location of the widget (in camera coordinates). We might need to transform that location into world coordinates to evaluate if it is accessible to the robot at all, and to gripper coordinates to determine when we should close the jaws of the gripper

Figure 2. A very simple robot arm with one joint and one gripper. The world, camera, joint, and gripper coordinate frames are indicated.

1.3 Transforming Points Between Coordinate Frames

Suppose that you know the location of a point in one coordinate frame (for example, airplane coordinates) and you want to know its location in another frame (for example, tower coordinates). How do you do it? We will start with a very simple case, and then move into more complex examples.

Let’s begin by considering the two coordinate systems in Figure 3, world coordinates and robot coordinates. Notice that the only difference between the two coordinate frames is that the robot frame has been translated by 3 units along the y axis from the world coordinate frame.

Figure 4 is a table of some sample points in world coordinates, and their corresponding values in robot coordinates. For the moment, ignore the third column of Figure 4, and just look at the first two columns. Notice that any point $\begin{bmatrix} a\ b\ c\ 1\\ \end{bmatrix}^{T}$ in world coordinates is the same as the point $\begin{bmatrix} a\ (b-3)\ c\ 1\\ \end{bmatrix}^{T}$ in robot coordinates. Similarly, any point $\begin{bmatrix} d\ e\ f\ 1\\ \end{bmatrix}^{T}$ in robot coordinates is the same as the point $\begin{bmatrix} d\ (e+3)\ f\ 1\\ \end{bmatrix}^{T}$ in world coordinates.

Figure 3. Two coordinate frames, world (w) and robot (r). The origin of the robot frame is located at the point (0,3,0) in world coordinates. Using our new notation, we can represent this as $\begin{bmatrix} 0\3\0\1\\ \end{bmatrix}^{T}$ . The two x axes are parallel, as are the two y and the two z axes. Note that the origin of the world coordinate axes is at $\begin{bmatrix} 0\ -3\0\1\\ \end{bmatrix}^{T}$ in robot coordinates.

We have seen that one way to convert world coordinates to robot coordinates for the system in Figure 3 is to subtract 3 from the y value in world coordinates. Surprisingly, another way to convert points from the world coordinate frame of Figure 14 to the robot coordinate frame of Figure 3 is to pre-multiply the point by the matrix $\begin{bmatrix} 1 \0 \0 \0\\ 0 \1 \0\ -3\\ 0 \0 \1 \0 \\ 0 \0 \0 \1 \\ \end{bmatrix}^{T}$ . The third column of Figure 15 does exactly this and results in the same answer!

It should be clear that there is nothing magic about the number -3 in our 4x4 matrix other than the fact that we moved 3 units along the y axis between the two frames. Indeed, if we had moved 63 units, then pre-multiplying by the matrix $\begin{bmatrix} 1 \0 \0 \0\\ 0 \1 \0\ -63\\ 0 \0 \1 \0 \\ 0 \0 \0 \1 \\ \end{bmatrix}^{T}$ would convert points from world coordinates to robot coordinates.

Figure 4. Converting between world and robot coordinates as depicted in Figure 3

Similarly, there’s nothing magic about the fact that we did this move along the y axis. We could come up with similar matrices for changes in frames that occurred along the y or z axis. Or indeed any combination of moves along all three matrices.

Suppose that to get from the coordinate frame p to the coordinate frame q you need to move a units along p’s x axis, b units along p’s y axis, and c units along p’s z axis. Then to take a point from q coordinates to p coordinates, you need to premultiply it by the matrix $\begin{bmatrix} 1\0\0\ a \\ 0\1\0\ b\\ 0\0\1\ c\\ 0\0\0\ 1\\ \end{bmatrix}^{T}$ For example, consider the two coordinate frames of Figure 5. To transform the robot coordinate frame into the world coordinate frame, you need to translate 5 units along the robot’s x axis, -4 units along the robot’s y axis, and -1 unit along the robot’s z axis. Thus to take a point in world coordinates and transform that into a point in robot coordinates, you need to premultiply that point by the matrix $\begin{bmatrix} 1\0\0\ 5 \\ 0\1\0\ -4\\ 0\0\1\ -1\\ 0\0\0\ 1\\ \end{bmatrix}^{T}$ . Some examples of this can be found in Figure 6.

Figure 5. Two coordinate frames that differ by only a translation. To get from car coordinates (c) to mountain coordinates (m) you must translate 5 units along the car’s x axis, -4 units along the car’s y axis, and -1 unit along the car’s z axis.

We call the matrix that converts a point from j coordinates to k coordinates the homogeneous transformation from j coordinates to k coordinates, and we abbreviate this as T^k_j. Figure 7 summarizes how to compute the matrix that converts between two frames that only differ by a translation

Figure 6. Converting points from the mountain coordinate frame of Figure 5 to the car coordinate frame of Figure 5.

Figure 7. Converting points between coordinate frames that only differ by a translation.

1.4 Coordinate Frames that Differ by a Rotation Around One Axis

Consider the two frames depicted in Figure 8. To transform the k coordinate frame into the j coordinate frame, we perform a rotation about k’s z axis by -90^o . By looking at Figure 8 (a), we can see that x_k = y_j , z_k = z_j , and x_j = -1*y_k. Let’s look at a few examples. The origin of the j axis in j coordinates: the point $\begin{bmatrix} 0\0\0\ 1 \\ \end{bmatrix}^{T}$ is the same as the origin of the j axis in k coordinates $\begin{bmatrix} 0\0\0\ 1 \\ \end{bmatrix}^{T}$ . The point $\begin{bmatrix} a\ b\ c\ 1 \\ \end{bmatrix}^{T}$ in j coordinates is located at $\begin{bmatrix} b\ -a\ c\ 1 \\ \end{bmatrix}^{T}$ in k coordinates. Again, there is a matrix that we can premultiply points in j coordinates by to transform them into points in k coordinates. If to transform the k coordinate frame into the j coordinate frame you rotate about the z axis by , then you can pre-multiply a point in j coordinates by the matrix $\begin{bmatrix} cos\theta \ -sin\theta \ 0\ 0 \\ -sin\theta \ cos\theta \ 0\ 0 \\ 0 \ 0 \ 1\ 0 \\ 0 \ 0 \ 0\ 1 \\ \end{bmatrix}$ . Let’s check this on the two examples that we’ve done already.

Figure 8. Two coordinate frames, j and k, that differ only by a rotation about the z axis. To transform the k coordinate frame into the j coordinate frame, we rotate by -90^o about k’s z axis.

In our example, $\theta$ is 90^o. cos -90^o = 0. sin -90^o = -1. So our matrix becomes $\begin{bmatrix} 0 \ 1 \ 0\ 0 \\ -1 \ 0 \ 0\ 0 \\ 0 \ 0 \ 1\ 0 \\ 0 \ 0 \ 0\ 1 \\ \end{bmatrix}$ . When we compute our matrix * $\begin{bmatrix} 0\0\0\ 1 \\ \end{bmatrix}^{T}$ we get $\begin{bmatrix} 0\0\0\ 1 \\ \end{bmatrix}^{T}$ as expected. When we multiply our matrix * $\begin{bmatrix} a\ b\ c\ 1 \\ \end{bmatrix}^{T}$ , we get $\begin{bmatrix} b \\ -a \\ c \\ 1 \\ \end{bmatrix}$ as expected.

At this point we know that we can design a matrix to convert points between two coordinate frames that only differ by a translation, or by a rotation about the z axis. It should not surprise you to learn that you can also design matrices to convert points between two coordinate frames that only differ by a rotation about the x or y axis too.

1.5 Putting it All Together

So far we have learned how to create the matrix that will compute the coordinates of a point in one coordinate frame given the coordinates of that point in another coordinate frame subject to the following condition: the two frames may only differ by a translation (along the 3 axes), or by a rotation about one axis. It is indeed possible to convert points between two coordinate frames that differ, perhaps by two translations, or by a rotation then a translation and then another rotation.

Figure 9. Summary of transformation matrices

The key to understanding how to do this is to understand that there are two ways to view any sequence of translations and rotations. The first way we will call “moving” coordinate systems. In moving coordinate systems, each step happens relative to the steps that have come before it. For example, Figure 10 shows the world and gripper coordinate frames for a particular robotic system. Note that not only is the gripper coordinate frame translated from the world coordinate frame, but there also must be some sort of rotation that caused the gripper’s x axis to point up instead of out of the page towards you as the world coordinates do.

In the “moving axes” approach, we say that to get from world coordinates to gripper coordinates, you need to do the following sequence of moves: Rotate about xw by -90 degrees. Call this new frame intermediate frame 1, and we’ll call its axes x ₁, y₁, and z₁. Next rotate about the new z₁ by -90 degrees. Call this new frame intermediate frame 2, and we’ll call its axes x₂, y₂, and z₂. Finally, translate by (0,0,5) relative to intermediate frame 2. This results in the gripper coordinate frame.

The alternative approach is the “fixed axes” approach. In this technique, all of your moves are relative to the original world coordinate frame. In the “fixed axes” approach, the picture is still as depicted in Figure 10, but this time the sequence of steps is: Rot x_w(-90) then Rot y_w (-90), then Trans(0,5,0) relative to world coordinates. Whether you choose to use the “moving axes” approach or the “fixed axes” approach, your final matrix will be the same. However the way you compute it will differ

1.6 Computing the Transformation Matrix Using Moving Axes

Figure 10. World and gripper coordinate frames for some robot. There are several ways we can think about transforming between the two coordinate frames.

Recall that the moving axes approach is as follows: to get from world coordinates to gripper coordinates, you need to do the following sequence of moves: Rotate about xw by -90 degrees. Call this new frame intermediate frame 1, and we’ll call its axes x1, y1, and z1. Next rotate about the new z1 by -90 degrees. Call this new frame intermediate frame 2, and we’ll call its axes x2, y2, and z2. Finally, translate by (0,0,5) relative to intermediate frame 2. This results in the gripper coordinate frame.

When we use “moving axes” we list the moves that we did from left to right, compute the individual matrices for each part, and then multiply them together. For example, in this situation, our sequence of equations is:

Rot x(-90) * Rot z(-90) * Trans(0,0,5)
The matrices for this product are as follows: $\begin{bmatrix} 1 \ 0 \ 0\ 0 \\ 0 \ 0 \ 1\ 0 \\ 0 \ -1 \ 0\ 0 \\ 0 \ 0 \ 0\ 1 \\ \end{bmatrix}$ $\begin{bmatrix} 0 \ 1 \ 0\ 0 \\ -1 \ 0 \ 0\ 0 \\ 0 \ 0 \ 1\ 0 \\ 0 \ 0 \ 0\ 1 \\ \end{bmatrix}$ $\begin{bmatrix} 1 \ 0 \ 0\ 0 \\ 0 \ 1 \ 0\ 0 \\ 0 \ 0 \ 1\ 5 \\ 0 \ 0 \ 0\ 1 \\ \end{bmatrix}$ = $\begin{bmatrix} 0& 1 & 0 &0 \\ 0& 0& 1&5 \\ 1& 0& 0& 0\\ 0& 0& 0&1 \end{bmatrix}$ The resulting matrix will transform a point from gripper coordinates to world coordinates. For example, consider the point (1, 2, 3) in gripper coordinates. We compute that in world coordinates by premultiplying by our new matrix as follows: $\begin{bmatrix} 1 \ 0 \ 0\ 0 \\ 0 \ 1 \ 0\ 0 \\ 0 \ 0 \ 1\ 5 \\ 0 \ 0 \ 0\ 1 \\ \end{bmatrix}$ $\begin{bmatrix} 1 \\ 2 \\ 3 \\ 1 \\ \end{bmatrix}$ = $\begin{bmatrix} 2 \\ 8 \\ 1 \\ 1 \\ \end{bmatrix}$ Which you should be able to verify is correct by looking at Figure 10.

1.7 Using Fixed Axes

Recall that the fixed axes approach does everything relative to the original world coordinate frame. The sequence of transformations in this case was as follows: Rot xw(-90) then Rot yw (-90), then Trans(0,5,0) relative to world coordinates.

When we using “fixed axes” computation, (i.e. each new rotation is relative to the original gripper coordinate frame), we write the equations from right to left.

Trans (0,5,0) * Rot y (-90) * Rot x(-90)

The matrices for this product are as follows:

$\begin{bmatrix} 1\0\0\0\\ 0\1\0\5\\ 0\0\1\0\\ 0\0\0\1\\ \end{bmatrix}$ $\begin{bmatrix} 0\0\ 1\ -0\\ 0\1\ 0\ 0\\ 1\0\ 0\ 0\\ 0\0\ 0\ 1\\ \end{bmatrix}$ $\begin{bmatrix} 1\ 0\ 0\0\\ 0\ 0\ 1\0\\ 0\ -1\ 0\0\\ 0\ 0\ 0\1\\ \end{bmatrix}$ = $\begin{bmatrix} 0\1\0\0\\ 0\0\1\5\\ 1\0\0\0\\ 0\0\0\1\\ \end{bmatrix}$

Check it out! this is the same equation we got when we did the computation using moving axes! (Phew!)

Previous Next