# Homogeneous Coordinates and Forward Kinematics

If you’re still following along after reading our last post, you’ll now have gotten the hang of what a frame of reference is, and how we can rotate and translate them relative to one another. Transforming one frame of reference to another is called a rigid body transformation.

But most motion in the real world consists of a combination of rotations and translations. How can we represent this kind of motion easily and efficiently? Well, it turns out that it’s not as hard as you might think.

There is a theory in kinematics that states that it is always possible to transform one reference frame to another, regardless of relative position and orientation, by a single rotation, followed by a translation .

This is easy to see when you consider that I can align any two frames of reference with one rotation, and then place them on top of each other with a translation.

So, if I have two arbitrarily positioned reference frames A and B, and I want to transform a vector from A to B, all I need to do is find the rotation and translation that maps these two frames:

$R_{OO'} \vec{x}_O +\vec{t}_{OO'}= \vec{x}_{O'}$

This is extremely useful for our work in understanding how robotic arms, as we’ll soon see.

# Homogenous Coordinates

This ability to map reference frames to each other is very useful, but it requires two distinct operations: Rotation, then translation. This means that we require a rotation matrix and a translation vector for every two reference frames that we want to relate.

Wouldn’t it be great if we could instead define a single matrix that completely represents the relationship between two reference frames? Well it turns out that we can, but to in order to do this, we need to change the kind of coordinates that we use to describe points in space.

Homogeneous coordinates are the new coordinates that we need to use, and they look exactly like our regular Cartesian coordinates, except that they have an extra 1 at the bottom:

$\begin{bmatrix} 5 \\ 7 \\ 9 \\ 1 \end{bmatrix}$

Why did we add the extra 1? Because it allows us to compose our rotation matrix and our translation vector into one, 4×4 matrix that represents both the rotation and the translation:

$T = \begin{bmatrix} R_{00} & R_{01} & R_{22} & t_{0} \\ R_{10} & R_{11} & R_{22} & t_{1} \\ R_{20} & R_{11} & R_{22} & t_{2} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$

We can now use this with our homogenous vectors to completely represent transformations between frames of reference:

$\begin{bmatrix} R_{00} & R_{01} & R_{22} & t_{0} \\ R_{10} & R_{11} & R_{22} & t_{1} \\ R_{20} & R_{11} & R_{22} & t_{2} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} x_{0} \\ x_{1} \\ x_{2} \\ 1 \end{bmatrix} = \begin{bmatrix} x_{0}\prime \\ x_{1}\prime \\ x_{2}\prime \\ 1 \end{bmatrix}$

Remember the $x\prime$ symbol represents the vector in the transformed coordinate frame.

Here is an example below, using the homogenous translation vector above, combined with a rotation matrix about the Z-axis:

$T = \begin{bmatrix} \cos(\theta) & -\sin(\theta) & 0 & 5 \\ \sin(\theta) & \cos(\theta) & 0 & 7 \\ 0 & 0 & 1 & 9 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$

The 1 allows us to treat the last column of the homogenous transformation matrix as a simple vector addition, which is the translation between the two frames.

With this small change, we can now represent the relationship between two reference frames with a single 4×4 matrix.

# Chaining Rigid Body Transformations

Suppose we have three reference frames, A, B and C. Using our new homogenous representation, let’s define two 4×4 rigid body transformation matrices: TAB and TBC. TAB maps vectors from reference frame A into frame B, and similarly, TBC maps vectors from reference frame B to frame C.

A question you might ask is: Given that I know a vector in frame A, can I transform it to frame C?

The answer is yes, we just need to chain our transformations:

$T_{BC} T_{AB} \vec{x}_A = \vec{x}_C$

This operation first maps our frame A vector to frame B, then maps the new calculated frame B vector to frame C using TBC. Even better, we can do this for arbitrarily long chains:

$T_{DE} T_{CD} T_{BC} T_{AB} \vec{x}_A = \vec{x}_E$

This will be the foundation for our next section.

# Forward Kinematics

Forward kinematics is essentially the question: Given that I know all of the joint angles of my robotic arm, what position is the end-effector in?

It will be useful to explain forward kinematics with an example. Take the robotic arm in a plane below:

We will walk you through solving the forward kinematics of this arm in 2D (just the plane). Remember, a robotic arm is a series of links, connected by joints, and each link has it’s own reference frame. Here we see a robotic arm with three links, and three joints.

It will be actually be useful to define and label our four reference frames here:

• The 0 frame (green), or the global reference frame. Often, this is just a reference frame that is attached to the base of the robot, or a table the robot sits on.
• The 1 frame (red), attached to the base of the first link (represented by L1 link)
• The 2 frame (blue), attached to the base of the second link (represented by L2 link)
• The 3 frame (cyan), or the end-effector frame, attached to the base of the third link (represented by L3 link)

To start, let’s look at what a rotation matrix looks like in 2D:

$\begin{bmatrix} \cos(\theta) & -\sin(\theta) \\ \sin(\theta) & \cos(\theta) \\ \end{bmatrix}$

This matrix describes a rotation of a vector by an angle $\theta$ in 2D. For each of our 3 joints, we can use it to describe the rotation of each link by it’s respective joint:

$R_{01} = \begin{bmatrix} \cos(\theta_1) & -\sin(\theta_1) \\ \sin(\theta_1) & \cos(\theta_1) \\ \end{bmatrix}$

$R_{12} = \begin{bmatrix} \cos(\theta_2) & -\sin(\theta_2) \\ \sin(\theta_2) & \cos(\theta_2) \\ \end{bmatrix}$

$R_{23} = \begin{bmatrix} \cos(\theta_3) & -\sin(\theta_3) \\ \sin(\theta_3) & \cos(\theta_3) \\ \end{bmatrix}$

Once we have rotated our initial reference frame, we must translate along the length of the next link to superimpose it on the next link’s reference frame. However, remember that we’ve rotated our reference frame, so to translate it we’ll need to calculate the translation vector from the rotated frame:

$T = -R \begin{bmatrix} L \\ 0 \\ \end{bmatrix} = \begin{bmatrix} -L\cos(\theta) \\ -L\sin(\theta) \\ \end{bmatrix}$

Remember, we are going to use homogeneous coordinates to allow us to represent the rotation and subsequent translation in one matrix:

$T_{01} = \begin{bmatrix} \cos(\theta_1) & -\sin(\theta_1) & 0 \\ \sin(\theta_1) & \cos(\theta_1) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}$

$T_{12} = \begin{bmatrix} \cos(\theta_2) & -\sin(\theta_2) &-L_1\cos(\theta_2) \\ \sin(\theta_2) & \cos(\theta_2) & -L_1\sin(\theta_2) \\ 0 & 0 & 1 \\ \end{bmatrix}$

$T_{23} = \begin{bmatrix} \cos(\theta_3) & -\sin(\theta_3) & -L_2\cos(\theta_3) \\ \sin(\theta_3) & \cos(\theta_3) & -L_2\sin(\theta_3) \\ 0 & 0 & 1 \\ \end{bmatrix}$

And that’s it! We can see what position our end-effector is now in by taking our joint angles, plugging them into our transformation matrices, and calculating. Just pick a vector in the global reference frame (the 0 frame) and feed it into the calculation to get the vector in the end-effector frame (the 3 frame):

$T_{23} T_{12} T_{01} \vec{x}_0 = \vec{x}_3$

# Next steps

Forward kinematics are a little more complicated in 3D, but are essentially the same. In our next post, we’ll work through the forward kinematics of a 3D robotic arm.