## Robot control part 1: Forward transformation matrices

I’m doing a tour of learning down at the Brains in Silicon lab run by Dr. Kwabena Boahen for the next month or so working on learning a bunch about building and controlling robots and some other things, and one of the super interesting things that I’m reading about is effective methods for force control of robots. I’ve mentioned operational space (or task space) control of robotic systems before, in the context of learning the inverse kinematic transformation, but down here the approach is to analytically derive the dynamics of the system (as opposed to learning them) and use these to explicitly calculate control signals to move in task space that take advantage of the passive dynamics of the system.

In case you don’t remember what those words mean, operational (or task) space refers to a different configuration space than the basic / default robot configuration space. FOR EXAMPLE: If we have a robot arm with three degrees of freedom (DOF), that looks something like this:

where two joints rotate, and are referred to as $q_1$, and $q_2$, respectively, then the most obvious representation for the system state, $\textbf{q}$, is the joint-angle space $\textbf{q} = [q_1, q_2]$. So that’s great, but often when we’re using this robot we’re going to be more interested in controlling the position of the end-effector rather than the angles of the joints. We would like to operate to complete our task in terms of $\textbf{q} = [x, y]$, where $x, y$ are the Cartesian coordinates of our hand in 2D space. So then $\textbf{q} = [x, y]$ is our operational (or task) space.

I also mentioned the phrase ‘passive dynamics’. It’s true, go back and check if you don’t believe me. Passive dynamics refer to how the system moves from a given initial condition when no control signal is applied. For example, passive dynamics incorporate the effects of gravity on the system. If we put our arm up in the air and remove any control signal, it falls down by our side. The reason that we’re interested in passive dynamics is because they’re movement for free. So if my goal is to move my arm to be down by my side, I want to take advantage of the fact that the system naturally moves there on it’s own simply by removing my control signal, rather than using a bunch of energy to force my arm to move down.

There are a bunch of steps leading up to building controllers that can plan trajectories in a task space. As a first step, it’s important that we characterize the relationship of each of reference coordinate frames of the robot’s links to the origin, or base, of the robot. The characterization of these relationships are done using what are called forward transformation matrices, and they will be the focus of the remainder of this post.

Forward transformation matrices in 2D

As I mentioned mere sentences ago, forward transformation matrices capture the relationship between the reference frames of different links of the robot. A good question might be ‘what is a reference frame?’ A reference frame is basically the point of view of each of the robotic links, where if you were an arm joint yourself what you would consider ‘looking forward’. To get a feel for these and why it’s necessary to be able to move between them, let’s look at the reference frames of each of the links from the above drawn robot:

We know that from $q_2$ our end-effector point $\textbf{p}$ is length $d_2$ away along its x-axis. Similarly, we know that $q_2$ is length $d_1$ away from $q_1$ along its x-axis, that $q_1$ is length $d_0$ away from the origin along its y-axis. The question is, then, in terms of the origin’s coordinate frame, where is our point $\textbf{p}?$

In this configuration pictured above it’s pretty straightforward to figure out, it’s simply $(x = d_2, y = d_0 + d_1)$. So you’re feeling pretty cocky, this stuff is easy. OK hotshot, what about NOW:

It’s not as straightforward once rotations start being introduced. So what we’re looking for is a method of automatically accounting for the rotations and translations of points between different coordinate frames, such that if we know the current angles of the robot joints and the relative positions of the coordinate frames we can quickly calculate the position of the point of interest in terms of the origin coordinate frame.

Accounting for rotation

So let’s just look quick at rotating axes. Here’s a picture:

The above image displays two frames of reference with the same origin rotated from each other by $q$ degrees. Imagine a point $\textbf{p} = (p_{x_1}, p_{y_1})$ specified in reference frame 1, to find its coordinates in terms of of the origin reference frame, or $(x_0, y_0)$ coordinates, it is necessary to find out the contributions of the $x_1$ and $y_1$ axes to the $x_0$ and $y_0$ axes. The contributions to the $x_0$ axis from $p_{x_1}$ are calculated

$cos(q) p_{x_1}.$

To include $p_{y_1}$‘s effect on position, we add the term

$cos(90 + q) p_{y_1},$

which is equivalent to $-cos(90 - q)$, as shown above. This term can be rewritten as $-sin(q)$ because $sin$ and $cos$ are phase shifted 90 degrees from one another.

The total contributions of a point defined in the $(x_1, y_1)$ axes to the $x_0$ axis are

$p_{0_x} = cos(q) p_{x_1} - sin(q) p_{y_1}.$

Similarly for the $y_0$ axis contributions we have

$p_{0_y} = sin(q) p_{x_1} + sin(90 - q) p_{y_1},$

$p_{0_y} = sin(q) p_{x_1} + cos(q) p_{y_1}.$

Rewriting the above equations in matrix form gives:

$^1_0\textbf{R} \; \textbf{p}_1 = \left[ \begin{array}{cc} cos(q_0) & -sin(q_0) \\ sin(q_0) & cos(q_0) \end{array} \right] \left[ \begin{array}{c} p_{x_1} \\ p_{y_1} \end{array} \right],$

where $^1_0\textbf{R}$ is called a rotation matrix.
The notation used here for these matrices is that the reference frame number being rotated from is denoted in the superscript before, and the reference frame being rotated into is in the subscript. $^1_0\textbf{R}$ denotes a rotation from reference frame 1 into reference frame 0 (using the same notation as described here.

To find the location of a point defined in reference frame 1 in reference frame 0 coordinates, we then multiply by the rotation matrix $^1_0\textbf{R}$.

Accounting for translation

Alrighty, rotation is great, but as you may have noticed our robots joints are not all right on top of each other. The second part of transformation is translation, and so it is also necessary to account for distances between reference frame origins.

Let’s look at the the reference frames 1 and 0 shown in the above figure, where point $\textbf{p} = (2,2)$ in reference frame 1. Reference frame 1 is rotated 45 degrees from and located at $(3, 2)$ in reference frame 0. To account for this translation and rotation a new matrix will be created that includes both rotation and translation. It is generated by appending distances, denoted $\textbf{D}$, to the rotation matrix $^1_0\textbf{R}$ along with a row of zeros ending in a 1 to get a transformation matrix:

$^1_0\textbf{T} = \left[ \begin{array}{cc} ^1_0\textbf{R} & ^1_0\textbf{D} \\ \textbf{0} & \textbf{1} \end{array} \right],$
$^1_0\textbf{T} = \left[ \begin{array}{ccc} cos(q_0) & -sin(q_0) & d_{x_0} \\ sin(q_0) & cos(q_0) & d_{y_0} \\ 0 & 0 & 1 \end{array} \right].$

To make the matrix-vector multiplications work out, a homogeneous representation must be used, which adds an extra row with a 1 to the end of the vector $\textbf{p}$ to give

$\textbf{p} = \left[ \begin{array}{c} p_x \\ p_y \\ 1 \end{array} \right].$

When position vector $\textbf{p}$ is multiplied by the transformation matrix $^1_0\textbf{T}$ the answer should be somewhere around $(3, 5)$ from visual inspection, and indeed:

$^1_0\textbf{T} \; \textbf{p} = \left[ \begin{array}{ccc} cos(45) & -sin(45) & 3 \\ sin(45) & cos(45) & 2 \\ 0 & 0 & 1 \end{array} \right] \left[ \begin{array}{c} 2 \\ 2 \\ 1 \end{array} \right] = \left[ \begin{array}{c} 3 \\ 4.8285 \\ 1 \end{array} \right].$

To get the coordinates of $\textbf{p}$ in reference frame 0 now simply take the first two elements of the resulting vector $\textbf{p} = (3, 4.8285)$.

Applying multiple transformations

We can also string these things together! What if we have a 3 link, planar (i.e. rotating on the $(x,y)$ plane) robot arm? A setup like this:

We know that our end-effector is at point $(1,2)$ in reference frame 2, which is at an 80 degree angle from reference frame 1 and located at $(x_1 = 2.5, y_1 = 4)$. That gives us a transformation matrix

$^2_1\textbf{T} = \left[ \begin{array}{ccc} cos(80) & -sin(80) & 2.5 \\ sin(80) & cos(80) & 4 \\ 0 & 0 & 1 \end{array} \right]$.

To get our point in terms of reference frame 0 we account for the transform from reference frame 1 into reference frame 2 with $^2_1\textbf{T}$ and then account for the transform from reference frame 0 into reference frame 1 with our previously defined transformation matrix

$^1_0\textbf{T} = \left[ \begin{array}{ccc} cos(45) & -sin(45) & 3 \\ sin(45) & cos(45) & 2 \\ 0 & 0 & 1 \end{array} \right]$.

So let’s give it a shot! By eyeballing it we should expect our answer to be somewhere around $(7,0)$ or so, I would say.

$\textbf{p}_0 = ^1_0\textbf{T} \; ^2_1\textbf{T} \; \textbf{p}_2 = \\ \\ \left[ \begin{array}{ccc} cos(45) & -sin(45) & 3 \\ sin(45) & cos(45) & 2 \\ 0 & 0 & 1 \end{array} \right] \left[ \begin{array}{ccc} cos(80) & -sin(80) & 2.5 \\ sin(80) & cos(80) & 4 \\ 0 & 0 & 1 \end{array} \right] \left[ \begin{array}{c} 1 \\ 2 \\ 1 \end{array} \right] = \left[ \begin{array}{c} -0.273 \\ 6.268 \\ 1 \end{array} \right]$.

And it’s a good thing we didn’t just eyeball it! Accurate drawings might have helped but the math gives us an exact answer. Super!

And one more note, if we’re often performing this computation, then instead of performing 2 matrix multiplications every time we can work out

$^2_0\textbf{T} = ^1_0\textbf{T} \; ^2_1\textbf{T}$

and simply multiply our point in reference frame 2 by this new transformation matrix $^2_0\textbf{T}$ to calculate the coordinates in reference frame 0.

Forward transform matrices in 3D

The example here is taken from Samir Menon’s RPP control tutorial.

It turns out it’s trivial to add in the $z$ dimension and start accounting for 3D transformations. Let’s say we have a standard revolute-prismatic-prismatic robot, which looks exactly like this, or roughly like this:

where the base rotates around the $z$ axis, and the distance from reference frame 0 to reference frame 1 is 1 unit, also along the $z$ axis. The rotation matrix from reference frame 0 to reference frame 1 is:

$^1_0\textbf{R} = \left[ \begin{array}{ccc} cos(q_0) & -sin(q_0) & 0 \\ sin(q_0) & cos(q_0) & 0 \\ 0 & 0 & 1 \end{array} \right]$

and the translation vector is

$^1_0\textbf{D} = [0, 0, 1]^T.$

The transformation matrix from reference frame 0 to reference frame 1 is then:

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

where the third column indicates that there was no rotation around the $z$ axis in moving between reference frames, and the forth (translation) column shows that we move 1 unit along the $z$ axis. The fourth row is again then only present to make the multiplications work out and provides no information.

For transformation from the reference frame 1 to reference frame 2, there is no rotation (because it is a prismatic joint), and there is translation along the $y$ axis of reference frame 1 equal to $.5 + q_1$. This gives a transformation matrix:

$^2_1\textbf{T} = \left[ \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0.5 + q_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right].$

The final transformation, from the origin of reference frame 2 to the end-effector position is similarly another transformation with no rotation (because this joint is also prismatic), that translates along the $z$ axis:

$^{ee}_2\textbf{T} = \left[ \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & -0.2 - q_2 \\ 0 & 0 & 0 & 1 \end{array} \right].$

The full transformation from reference frame 0 to the end-effector is found by combining all of the above transformation matrices:

$^{ee}_0\textbf{T} = ^1_0\textbf{T} \; ^2_1\textbf{T} \; ^{ee}_2\textbf{T} = \left[ \begin{array}{cccc} cos(q_0) & -sin(q_0) & 0 & -sin(q_0)(0.5 + q_1) \\ sin(q_0) & cos(q_0) & 0 & cos(q_0) (0.5 + q_1) \\ 0 & 0 & 1 & 0.8 - q_2 \\ 0 & 0 & 0 & 1 \end{array} \right].$

To transform a point from the end-effector reference frame into terms of the origin reference frame, simply multiply the transformation matrix by the point of interest relative to the end-effector. If it is the end-effector position that is of interest to us, $p = [0, 0, 0, 1]^T$. For example, let $q_0 = \frac{\pi}{3}$, $q_1 = .3$, and $q_2 = .4$, then the end-effector location is:

$^{ee}_0\textbf{T} \; \textbf{p} = \left[ \begin{array}{cccc} cos(q_0) & -sin(q_0) & 0 & -sin(q_0)(0.5 + q_1) \\ sin(q_0) & cos(q_0) & 0 & cos(q_0) (0.5 + q_1) \\ 0 & 0 & 1 & .8 + q_2 \\ 0 & 0 & 0 & 1 \end{array} \right] \; \left[ \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right] = \left[ \begin{array}{c} -0.693 \\ 0.4 \\ 0.8 \\ 1 \end{array} \right].$

Inverting our transformation matrices

What if we know where a point is defined in reference frame 0, but we want to know where it is relative to our end-effector’s reference frame? Fortunately this is straightforward thanks to the way that we’ve defined our transform matrices. Continuing the same robot example and configuration as above, and denoting the rotation part of the transform matrix $\textbf{R}$ and the translation part $\textbf{D}$, the inverse transform is defined:

$(^{ee}_0\textbf{T})^{-1} = \left[ \begin{array}{cc} (^{ee}_0\textbf{R})^T & -(^{ee}_0\textbf{R})^T \; ^{ee}_0\textbf{D} \\ 0 & 1 \end{array} \right].$

If we have a point that’s at $\textbf{p}_0 = [1, 1, .5, 1]^T$ in reference frame 0, then we can calculate that relative to the end-effector it is at:

$\textbf{p} = (^{ee}_0\textbf{T})^{-1} \; \textbf{p}_0 = [1.37, -1.17, -0.3, 1]^T.$

Conclusions

These are, of course, just the basics with forward transformation matrices. There are numerous ways to go about this, but this method is fairly straightforward. If you’re interested in more, there are a bunch of youtube videos and detailed tutorials all over the web. There’s a bunch of neat stuff about why the matrices are set up like they are (search: homogeneous transformations) and more complex examples.

The robot example for the 3D case here didn’t have any spherical joints, each joint only moved in 2 dimensions, but it is also possible to derive the forward transformation matrix in this case, it’s just more complex and not necessary to move onward here since they’re not used in the robots I’ll be looking at. This introduction is enough to get started and move on to some more exciting things, so let’s do that!

## 32 thoughts on “Robot control part 1: Forward transformation matrices”

1. […] where the end-effector is relative to a base coordinate frame…OH MAYBE we should use those forward transformation matrices from the previous post. Let’s do […]

2. […] Robot control part 1: Forward transformation matrices. […]

3. Reblogged this on Exoskeleton Project Blog.

4. Hey, thank you for the great posts! I think there is a technical mistake on this http://bit.ly/19xGQTg , because point p2 has coordinates (1,2)

Good luck! Great job with the blog 🙂

5. gavand uttam says:

thanks for explaining robot matrices. i was strugulling from many days

• travisdewolf says:

You’re welcome! I similarly had quite a time figuring them out when I first came across them. 🙂

• jojo says:

Hey,

There are some things i still done understand. Why do some sources of information say that to go from v to v’ you use

cos -sin
sin cos

which will mean the above matrix rotation would be 0R1 not 1R0 as you said above.

Also, the Transformation matrix seems to be moving points from frame 0 to frame 1 not frame 1 to frame 0.

The logic seems really counter intuitive because it is backwards from information posted on sites like wolframalpha

Thanks

• travisdewolf says:

Myyyy goodness, that’s a long standing mistake.
Thank you for this catch, you’re correct, I’ll fix this!
I also updated the notation to some that’s hopefully easier to read.

• Peter says:

Hi, AFAIK the last edit (in response to @jojo) introduced an error. The following rotaition matrix (lets call it M)
cos -sin
sin cos
is definitly 1R0. M rotates a VECTOR conterclock-wise, this is equivalent to rotating the COORDINATESYSTEM clockwise. By the notation of your diagrams this would mean going from 1 to 0 and thus M = 1R0

• travisdewolf says:

Hi Peter,

Aha, this is a result of me messing up the notation, again. Thanks for catching this, I’ll fix it!

6. CuPi says:

I want to ask you about T12 transformation from the reference frame 2 to reference frame 1. Why theta1 is translated along the y axis. I think that theta1 is translate along x axis.
So, T12 = [1 0 0 theta1;
0 1 0 0.5;
0 0 1 0;
0 0 0 1];
can you explain it to me? thanks

• travisdewolf says:

Hi CuPi, if the axes were defined more intuitively you would be correct. But in this example the Z axis is up/down along the picture, the Y axis is left/right, and the X axis comes out/into the picture. It’s just because of this definition of the axes that the transformation matrix isn’t set up how you have above!

edit: I should be more explicit about this in the post, I’ll edit it to make it more clear. Thanks!

7. ml1991 says:

Reblogged this on meng91 and commented:
detail and useful

8. […] We do that in the joints_x and joints_y variables, and the trig for that is straight from basic robotics. The above code results in the following animation (which is a little choppy because I dropped some […]

9. […] this was a time I was very glad to have a previous post talking about generating transformation matrices, because deriving the Jacobian for a 6DOF arm in 3D space comes off as a little daunting when […]

10. watson says:

hi i understand it now but just a question on how u got the 0.5+q1 in matrix for T21

• travisdewolf says:

Hi watson, yeah that is not clear! Sorry about that, the .5 is the offset of q1’s origin along the y-axis of q0’s reference frame. Similarly, there is a -.2 offset of q2 along the z-axis of q1 reference frame. Does that help? I’ll update the figures asap to hopefully make it more clear where those come from!

11. its a nice piece of information .. like to see you more ..

12. […] For this next chunk I’m going to cut out everything that’s not VREP, since I have a bunch of posts explaining the control signal derivation and forward transformation matrices. […]

• travisdewolf says:

Howdy! You’re welcome, glad you found it useful!
The last column in the matrix represents the translations along the (x,y,z) axes. They’re calculated by finding the translation between each of the coordinate frames individually, generating the transformation matrices, and then multiplying them all together. The 1 in the last row doesn’t represent anything, it’s just there to make all the calculations work out when you multiply. Does that help?

13. […] go over how to calculate the inverse transform at the end of my post on forward transformation matrices, but to save you from having to go back and look through that, here’s the code to do […]

14. jwhendy says:

One suggestion is just to show the other angle of the triangle we’re using to calculate the rotation matrix. So instead of citing that -sin(theta – 90) is really cos(theta) and similar, what about just showing that the other angle *is* theta and work with that. Some folks might be quicker/more comfortable with thinking “opposite = sin, adjacent = cos” vs. figuring out sign flips for f(theta – 90). Just an idea/alternative!

• travisdewolf says:

ah that’s a good idea! one day i’ll go back and rework these / clean them up some more, i’ll make sure to incorporate this! thanks!

15. Randy says:

Thanks for this blog post, it’s super helpful!

16. uda says:

Hi Travis. Thanks for the wonderful tutorial.
I am revising my basics and it seems like there is a little typo in your tutorial. In
py0 = cos(90 + q) py1 , shouldn’t py0 actually be px0?
Happy to be corrected.

• travisdewolf says:

Hi Uda, thanks for the catch! I can’t tell if I made a typo or committed to some horrible notation, but either way it’s unclear and I’ll update it. Thanks!

17. magpie says:

This is hands down the best article on robot matrices! Thank you for publishing it!