Deriving a robot’s transform matrices

While doing some soul searching recently, I realised that in previous posts I’ve glossed over actually deriving transform matrices, and haven’t discussed a methodical way of going about it. If you’ve ever tried working out transforms you know they can be a pain if you don’t know have a set process to follow. Although I’ve given a bunch of examples in previous posts, this post is intended to clear up any confusion that crops up when you’re trying to work out the transforms for your own robot. This is far from an absolute guide covering all the cases, but hopefully it gives a firm enough footing that you can handle the rest.

Note: I’m only going to be dealing with revolute joints in this post. Linear joints are easy (just put the joint offset in the translation part of the transform matrix) and spherical joints are ~~horrible death~~ beyond the scope of this post.

First thing’s first, quick recap of what transform matrices actually are. A transform matrix changes the coordinate system (reference frame) a point is defined in. We’re using the notation $\textbf{T}_0^1$ to denote a transformation matrix that transforms a point from reference frame 1 to reference frame 0. To calculate this matrix, we described the transformation from reference frame 0 to reference frame 1.

To make things easier, we’re going to break up each transform matrix into two parts. Unfortunately I haven’t found a great way to denote these two matrices, so we’re going to have to use additional subscripts:

$\textbf{T}^{i+1}_{ia}$ : accounting for the joint rotation, and
$\textbf{T}^{i+1}_{ib}$ : accounting for static translations and rotations.

So the $\textbf{T}_a$ matrix accounts for transformations that involve joint angles, and the $\textbf{T}_b$ matrix accounts for all the static transformations between reference frames.

Step 1: Account for the joint rotation

When calculating the transform matrix from a joint to a link’s centre-of-mass (COM), note that the reference frame of the link’s COM rotates along with the angle of the joint. Conversely, the joint’s reference frame does not rotate with the joint angle. Look at this picture!

In these pictures, we’re looking at the transformation from joint i’s reference frame to the COM for link i. $q_i$ denotes the joint angle of joint i. In the first image (on the left) the joint angle is almost 90 degrees, and on the right it’s closer to 45 degrees. Keeping in mind that the reference frame for COM i changes with the joint angle, and the reference frame for joint i does not, helps make deriving the transform matrices much easier.

So, to actually account for joint rotation, all you have to do is determine which axis the joint is rotating around and create the appropriate rotation matrix inside the transform.

For rotations around the x axis:

$\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \textrm{cos}(q_i) & -\textrm{sin}(q_i) & 0 \\ 0 & \textrm{sin}(q_i) & \textrm{cos}(q_i) & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$

For rotations around the y axis:

$\left[\begin{array}{cccc} \textrm{cos}(q_i) & 0 & -\textrm{sin}(q_i) & 0 \\ 0 & 1 & 0 & 0 \\ \textrm{sin}(q_i) & 0 & \textrm{cos}(q_i) & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$

For rotations around the z axis:

$\left[\begin{array}{cccc} \textrm{cos}(q_i) & -\textrm{sin}(q_i) & 0 & 0 \\ \textrm{sin}(q_i) & \textrm{cos}(q_i) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$

And there you go! Easy. The $\textbf{T}_a$ should be one of the above matrices unless something fairly weird is going on. For the arm in the diagram above, the joint is rotating around the $z$ axis, so

$\textbf{T}_a = \left[\begin{array}{cccc} \textrm{cos}(q_i) & -\textrm{sin}(q_i) & 0 & 0 \\ \textrm{sin}(q_i) & \textrm{cos}(q_i) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$

Step 2: Account for translations and any static axes rotations

Once the joint rotation is accounted for, translations become much simpler to deal with: Just put joint i at 0 degrees and the offset from the origin of reference frame i to i+1 is your translation.

So for the above example, this is what the system looks like when joint i is at 0 degrees:

where I’ve also added labels to the $x$ and $y$ axes for clarity. Say that the COM is at $(1, 0)$ , then that is what you set the $x, y$ translation values to in the $\textbf{T}_b$ .

Also we need to note that there is a rotation in the axes between reference frames (i.e. $x$ and $y$ do not point in the same direction for both axes sets). Here the rotation is 90 degrees. So the rotation part of the transformation matrix should be a 90 degree rotation.

Thus, the transformation matrix for our static translations and rotations is

$\textbf{T}_b = \left[\begin{array}{cccc} 0 & -1 & 0 & 1 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]$

Step 3: Putting them together

This is pretty easy, to compute the full transform from joint i to the COM i you just string together the two matrices derived above:

$\textbf{T}^{COM_i}_{joint_i} = \textbf{T}_a \textbf{T}_b$

where I’m using simplified notation on the left-hand side because we don’t need the full notation for clarity in this simple example.

So when you’re transforming a point, you left multiply by $\textbf{T}$ :

$\textbf{x}_{joint_i} = \textbf{T}^{COM_i}_{joint_i} \textbf{x}_{COM_i} = \textbf{T}_a \textbf{T}_b \textbf{x}_{COM_i},$

which is not great notation, but hopefully conveys the idea. To get our point $\textbf{x}$ into the reference frame of joint i, we left multiply the point as defined in the reference frame of COM i by $\textbf{T}^{COM_i}_{joint_i}$ .

So there you go! That’s the process for a single joint to COM transform. For transforms from COM to joints it’s even easier because you only need to account for the static offsets and axes rotations.

Examples

It’s always nice to have examples, so let’s work through the deriving the transform matrices for the Jaco2 arm as it’s set up in the VREP. The process is pretty straight-forward, with really the only tricky bit converting from the Euler angles that VREP gives you to a rotation matrix that we can use in our transform.

But first thing’s first, load up VREP and drop in a Jaco2 model.

NOTE: I’ve renamed the joints and links to start at 0, to make indexing in / interfacing with VREP easier.

We’re going to generate the transforms for the whole arm piece by piece, sequentially: origin -> link 0 COM, link 0 COM -> joint 0, joint 0 -> link 1 COM, etc. I’m going to use the notation $l_i$ and $j_i$ to denote link i COM and joint i, respectively, in the transform sub and superscripts.

The first transform we want then is for origin -> link 0 COM. There’s no joint rotation that we need to account for in this transform, so we only have to look for static translation and rotation. First click on link 0, and then

‘Object/item shift’ button from the toolbar,
the ‘Position’ tab,
select the ‘Parent frame’ radio button,

and it will provide you with the translation of link 0’s COM relative to its parent frame, as shown below:

So the translation part of our transformation is $[0, 0, .0784]^T$ .

The rotation part we can see by

‘Object/item rotate’ button from the toolbar,
the ‘Orientation’ tab,
select the ‘Parent frame’ radio button,

Here, it’s not quite as straight forward to use this info to fill in our transform matrix. So, if we pull up the VREP description for their Euler angles we see that to generate the rotation matrix from them you perform:

$\textbf{R} = \textbf{R}_x(\alpha) \textbf{R}_y(\beta) \textbf{R}_z(\gamma)$

where the $\textbf{R}_i$ matrix is a rotation matrix around the $i$ axis (as described above). For $(\alpha=0, \beta=0, \gamma=0)$ this works out to no rotation, as you may have guessed. So then our first transform is

$\textbf{T}^{l0}_{orgin} = \left[ \begin{array}{cccc}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & .0784 \\ 0 & 0 & 0 & 1\end{array} \right]$

For the second transform matrix, from link 0 to joint 0, again we only have to account for static translations and rotations. Pulling up the translation information:

vrep03
we have the translation $[0, 0, 0.0784]^T$ .

And this time, for our rotation matrix, there’s a flip in the axes so we have something more interesting than no rotation:

vrep04
we have Euler angles $(-180, 0, 0)$ . This is also a good time to note that the angles are provided in degrees, so let’s convert those over, giving us approximately $(\pi, 0, 0)$ . Now, calculating our rotation matrix:

$\textbf{R} = \textbf{R}_x(\alpha) \textbf{R}_y(\beta) \textbf{R}_z(\gamma)$

$\textbf{R}^{j0}_{l0} = \left[ \begin{array}{ccc}1 & 0 & 0 \\ 0 & \textrm{cos}(\pi) & -\textrm{sin}(\pi) \\ 0 & \textrm{sin}(\pi) & \textrm{cos}(\pi) \end{array} \right] \left[ \begin{array}{ccc}\textrm{cos}(0) & 0 & -\textrm{sin}(0) \\ 0 & 1 & 0 \\ \textrm{sin}(0) & 0 & \textrm{cos}(0) \end{array} \right] \left[ \begin{array}{ccc} \textrm{cos}(0) & -\textrm{sin}(0) & 0 \\ \textrm{sin}(0) & \textrm{cos}(0) & 0 \\ 0 & 0 & 1 \end{array} \right]$

$\textbf{R}^{j0}_{l0} = \left[ \begin{array}{ccc}1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & -1 \end{array} \right]$

which then gives us the transform matrix

$\textbf{T}^{j0}_{l0} = \left[ \begin{array}{cccc}1 & 0 & 0 & 0 \\ 0 & -1 & 0 & 0\\ 0 & 0 & -1 & 0.0784 \\ 0 & 0 & 0 & 1\end{array} \right].$

And the last thing I’ll show here is accounting for the joint rotation. In the transform from joint 0 to link 1 we have to account for the joint rotation, so we’ll break it down into two matrices as described above. To generate the first one, all we need to know is which axis the joint rotates around. In VREP, this is almost always the $z$ axis, but it’s good to double check in case someone has built a weird model. To check, one easy way is to make the joint visible, and the surrounding arm parts invisible:

vrep05
You can do this by

double clicking on the joint in the scene hierarchy to get to the Scene Object Properties window,
selecting the ‘Common’ tab,
selecting or deselecting check boxes from the ‘Visibility’ box.

As you can see in the image, this joint is indeed rotating around the $z$ axis, so the first part of our transformation from joint 0 to link 1 is

$\textbf{T}^{l1}_{j0a} = \left[ \begin{array}{cccc}\textrm{cos}(q_0) & -\textrm{sin}(q_0) & 0 & 0 \\ \textrm{sin}(q_0) & \textrm{cos}(q_0) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array} \right]$

The second matrix can be generated as previously described, so I’ll leave that, and the rest of the arm, to you!

If you’d like to see a full arm example, you can check out the tranforms for the UR5 model in VREP up on my GitHub.

5 thoughts on “Deriving a robot’s transform matrices”

atomoclast says:

June 9, 2017 at 11:29 am

I’ve been following your blog for a while, and you’ve partially inspired me to start my own. I’m in the process of writing a post very similar to this right now that shows the math for Forward Kinematics and Inverse Kinematics and how to do it with ROS.

I was wondering if I could reference some of your math in that post as well.

- travisdewolf says:
  
  June 9, 2017 at 12:39 pm
  
  That’s awesome! Sounds like your post will be great reference material / useful. My goal with the blog is just to spread / share information, so please borrow / use anything that’s helpful from any posts. If you find a space to give me a shout out, too, that’d be great. 🙂
  
  - travisdewolf says:
    
    June 9, 2017 at 12:40 pm
    
    Oh, and if you could let me know whenever your post is out I’d be interested to read it! 😀
ABR Control 0.1 repo public release! | studywolf says:

July 1, 2017 at 8:27 am

[…] Automates generation of functions for computing the Jacobians, joint space and task space inertia matrices, centrifugal and Coriolis effects, and Jacobian derivative, provided each link’s inertia matrix and the transformation matrices […]

Force control of task-space orientation | studywolf says:

December 3, 2018 at 4:38 pm

[…] Rotation matrices specify 3 orthogonal unit vectors, and I describe in detail how to calculate them in this post on forward transforms. And quaternions are 4-dimensional vectors used to describe 3 dimensional orientation, which […]

studywolf

a blog for things I encounter while coding and researching neuroscience, motor control, and learning