## 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:

1. $\textbf{T}^{i+1}_{ia}$: accounting for the joint rotation, and
2. $\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:

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:

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:

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.

## 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!