Skip to article frontmatterSkip to article content
Site not loading correctly?

This may be due to an incorrect BASE_URL configuration. See the MyST Documentation for reference.

Chapter 4 — Homogeneous Transformation Matrices

In Chapter 3 we found where the tip of a 2R arm is by drawing triangles and using trigonometry. That works for two links in a plane, but it gets messy fast: add a third link, or lift the arm into 3D, and you have to start the geometry over.

This chapter gives us a systematic tool instead. The idea:

  1. Glue a coordinate frame to every link.

  2. Describe each frame only relative to the frame one step before it — a small, local question.

  3. Store each answer as one matrix, the homogeneous transformation matrix (HTM), and chain the steps together by plain matrix multiplication.

Chapter 5 turns this into a recipe for the forward kinematics of any robot arm, and Chapter 6 reuses the same frames to get velocities.

After this chapter you should be able to:

  • write the rotation matrix R10\mathbf{R}_1^0 of a frame by looking at a drawing;

  • convert the coordinates of a point from one frame to another;

  • build the HTM T10\mathbf{T}_1^0 and use it to move points between frames;

  • chain HTMs to get the tip of a 2R arm, and invert an HTM.

SectionTopic
4.1Coordinate frames — why robots need them
4.2Posture in 2D — position, rotation matrix, 2D HTM, the 2R arm
4.3Orientation in 3D — the 3D rotation matrix, combining rotations, other representations
4.4The HTM in 3D — structure, composition, inverse
4.5Chapter summary and practice problems

Notation (the same as in Chapters 5 and 6)

SymbolMeaning
{0},{1},{2},…\{0\}, \{1\}, \{2\}, \dotscoordinate frames; {0}\{0\} is fixed to the ground
x1,y1,z1x_1, y_1, z_1 and o1o_1the axes and the origin of frame {1}\{1\}
q0\mathbf{q}^0coordinates of a point qq expressed in frame {0}\{0\}
o10\mathbf{o}_1^0the origin of frame {1}\{1\}, expressed in frame {0}\{0\} (position)
x10,y10,z10\mathbf{x}_1^0, \mathbf{y}_1^0, \mathbf{z}_1^0unit vectors along x1,y1,z1x_1, y_1, z_1, expressed in frame {0}\{0\}
R10\mathbf{R}_1^0rotation matrix: orientation of frame {1}\{1\} relative to frame {0}\{0\}
T10\mathbf{T}_1^0HTM: posture (position and orientation) of frame {1}\{1\} relative to frame {0}\{0\}
q~0\tilde{\mathbf{q}}^0q0\mathbf{q}^0 with a 1 added at the bottom (“augmented” coordinates)

How to read the indices: the subscript names the frame being described, the superscript names the frame we are looking from. So T10\mathbf{T}_1^0 is “frame 1, seen from frame 0”. In code we write T10 for T10\mathbf{T}_1^0 and R10 for R10\mathbf{R}_1^0 — the same naming as Chapter 5.

Colors in every plot: xx axis red, yy axis green, zz axis blue.

4.1 Coordinate Frames — Why Robots Need Them

Giving directions, robot-style

When you give a friend directions to a coffee shop you don’t say “it is 214 m east and 118 m north of the North Pole.” You say “walk out the door, go two blocks, turn left, it’s the second building on your right.” Each instruction is relative to where the previous instruction left you. A coordinate frame is exactly that: a local point of view that nearby things are measured from.

What is a coordinate frame?

A frame is an origin oo plus perpendicular unit-length axes — x,yx, y in 2D, and x,y,zx, y, z in 3D. Once a frame is chosen, “where is this point?” has a precise answer: its coordinates in that frame.

A robot arm is a chain of rigid links connected by joints. We glue a frame to every link, so the frame moves with the link (lecture notes, Lecture 5):

  • Frame {0}\{0\}: fixed to the base (the ground). It never moves.

  • Frame {1}\{1\}: attached to link 1, moves with joint 1.

  • Frame {2}\{2\}: attached to link 2, moves with joints 1 and 2.

  • … and so on, up to frame {n}\{n\} at the last link.

The big idea: describe one step at a time

Let Tii−1\mathbf{T}_i^{i-1} represent the posture (position and orientation) of frame {i}\{i\} relative to frame {i−1}\{i-1\} — one step back along the chain. This is an easy, local question: it only depends on the joint and the link between those two frames. Then the posture of the last frame relative to the ground is just the product

Tn0=T10 T21 T32⋯Tnn−1.\mathbf{T}_n^0 = \mathbf{T}_1^0\,\mathbf{T}_2^1\,\mathbf{T}_3^2\cdots\mathbf{T}_n^{n-1}.

Notice how the indices “cancel” between neighbors (10 21→20^0_1\,{}^1_2 \to {}^0_2). This works for 2 links or 20, in 2D or 3D. The rest of the chapter builds the matrix T\mathbf{T} that makes this possible.

Figure 4.1 — One frame per link. Chaining the one-step postures gives the last frame relative to the ground.

The widget below shows frames {0}\{0\}, {1}\{1\}, {2}\{2\} on the 2R arm from Chapter 3, placed as in the lecture notes: frame {1}\{1\} at the elbow with x1x_1 along link 1, and frame {2}\{2\} at the tip with x2x_2 along link 2. Move the sliders: frame {1}\{1\} only reacts to θ1\theta_1, while frame {2}\{2\} is carried by both joints.

Loading...

Key Takeaways — Section 4.1

  • A coordinate frame = an origin + perpendicular unit axes.

  • Attach one frame to each link; the frame moves with its link.

  • Describe each frame relative to the one before it, Tii−1\mathbf{T}_i^{i-1}, then chain: Tn0=T10 T21⋯Tnn−1\mathbf{T}_n^0 = \mathbf{T}_1^0\,\mathbf{T}_2^1\cdots\mathbf{T}_n^{n-1}.

4.2 Posture in 2D

The posture of frame {1}\{1\} relative to frame {0}\{0\} has two parts:

  • position — where is the origin o1o_1?

  • orientation — which way do the axes x1,y1x_1, y_1 point?

Figure 4.2 — Posture of frame {1} relative to frame {0}: position o10 and orientation θ.

4.2.1 Position

The position is the vector from o0o_0 to o1o_1, written in frame {0}\{0\}:

o10=[63]← 6 = x coordinate in frame {0},  3=y coordinate in frame {0}.\mathbf{o}_1^0 = \begin{bmatrix} 6 \\ 3 \end{bmatrix} \quad\leftarrow\ \text{6 = $x$ coordinate in frame } \{0\},\ \ 3 = y \text{ coordinate in frame } \{0\}.

(Seen from frame {1}\{1\} itself, its own origin is always o11=[0, 0]⊤\mathbf{o}_1^1 = [0,\ 0]^\top. The superscript matters!)

4.2.2 Orientation: the 2D rotation matrix

Frame {1}\{1\} is turned counter-clockwise by θ\theta relative to frame {0}\{0\}. We record its orientation by writing its own axes in frame {0}\{0\}, side by side:

R10=[x10y10],x10=unit vector along x1, expressed in frame {0}y10=unit vector along y1, expressed in frame {0}\mathbf{R}_1^0 = \begin{bmatrix}\mathbf{x}_1^0 & \mathbf{y}_1^0\end{bmatrix}, \qquad \begin{aligned} \mathbf{x}_1^0 &= \text{unit vector along } x_1 \text{, expressed in frame } \{0\}\\ \mathbf{y}_1^0 &= \text{unit vector along } y_1 \text{, expressed in frame } \{0\} \end{aligned}

From the drawing, the unit vector x1x_1 has a component cos⁡θ\cos\theta along x0x_0 and sin⁡θ\sin\theta along y0y_0; the unit vector y1y_1 has −sin⁡θ-\sin\theta along x0x_0 and cos⁡θ\cos\theta along y0y_0. So

R10=[cos⁡θ−sin⁡θsin⁡θcos⁡θ].\mathbf{R}_1^0 = \begin{bmatrix}\cos\theta & -\sin\theta \\ \sin\theta & \cos\theta\end{bmatrix}.

Another way to get the entries — dot products. Recall the inner (dot) product A⋅B=∥A∥ ∥B∥cos⁡θ\mathbf{A}\cdot\mathbf{B} = \|\mathbf{A}\|\,\|\mathbf{B}\|\cos\theta. If B\mathbf{B} is a unit vector, A⋅B\mathbf{A}\cdot\mathbf{B} is simply the length of the shadow of A\mathbf{A} on B\mathbf{B}, i.e. the component of A\mathbf{A} along B\mathbf{B}. So every entry of the rotation matrix is “how much of one axis lies along another”:

R10=[x1⋅x0y1⋅x0x1⋅y0y1⋅y0].\mathbf{R}_1^0 = \begin{bmatrix} x_1\cdot x_0 & y_1\cdot x_0 \\ x_1\cdot y_0 & y_1\cdot y_0 \end{bmatrix}.

Figure 4.3 — The dot product with a unit vector is the length of the shadow (projection).

Column 1 is x1x_1 seen from frame {0}\{0\}, column 2 is y1y_1 seen from frame {0}\{0\}. Move the slider and watch the printed columns follow the red and green arrows.

Loading...

4.2.3 A rotation matrix converts coordinates between frames

Here is the main job of a rotation matrix (Lecture 6). Frames {0}\{0\} and {1}\{1\} share an origin; x1x_1 points along y0y_0 and y1y_1 points along −x0-x_0 (frame {1}\{1\} is turned by 90∘90^\circ).

Figure 4.4 — Lecture 6 example: the same point p described in frame {0} and in frame {1}.

Read the columns off the drawing:

R10=[x10y10]=[0−110].\mathbf{R}_1^0 = \begin{bmatrix}\mathbf{x}_1^0 & \mathbf{y}_1^0\end{bmatrix} = \begin{bmatrix}0 & -1\\ 1 & 0\end{bmatrix}.

A point pp sits at 1 along x0x_0 and 1 along y0y_0, so p0=[1, 1]⊤\mathbf{p}^0 = [1,\ 1]^\top. Measured along frame {1}\{1\}'s axes it is 1 along x1x_1 and -1 along y1y_1, so p1=[1, −1]⊤\mathbf{p}^1 = [1,\ -1]^\top. The rotation matrix connects the two:

p0=R10 p1=[0−110][1−1]=[0⋅1+(−1)(−1)1⋅1+0⋅(−1)]=[11].✓\mathbf{p}^0 = \mathbf{R}_1^0\,\mathbf{p}^1 = \begin{bmatrix}0 & -1\\ 1 & 0\end{bmatrix}\begin{bmatrix}1\\-1\end{bmatrix} = \begin{bmatrix}0\cdot1 + (-1)(-1)\\ 1\cdot1 + 0\cdot(-1)\end{bmatrix} = \begin{bmatrix}1\\1\end{bmatrix}. \checkmark

Going the other way. Swap the roles of the frames: R01=[x01  y01]\mathbf{R}_0^1 = [\mathbf{x}_0^1\ \ \mathbf{y}_0^1] holds frame {0}\{0\}'s axes seen from frame {1}\{1\}. From the drawing, x0x_0 points along −y1-y_1 and y0y_0 along x1x_1:

R01=[01−10]=(R10)⊤,p1=R01 p0=[1−1].✓\mathbf{R}_0^1 = \begin{bmatrix}0 & 1\\ -1 & 0\end{bmatrix} = \left(\mathbf{R}_1^0\right)^\top, \qquad \mathbf{p}^1 = \mathbf{R}_0^1\,\mathbf{p}^0 = \begin{bmatrix}1\\-1\end{bmatrix}. \checkmark

So to go back, just transpose — no matrix inversion needed.

Why is the inverse just the transpose? (click to expand)

The entries of \mathbf{R}_1^0 are dot products: row i, column j is (axis j of frame 1)\cdot(axis i of frame 0). For \mathbf{R}_0^1 the roles swap, which swaps rows and columns — a transpose. Since going 0\to1\to0 must change nothing, \mathbf{R}_0^1\mathbf{R}_1^0 = \mathbf{I}, i.e. (\mathbf{R}_1^0)^{-1} = (\mathbf{R}_1^0)^\top.

Three properties every rotation matrix has:

PropertyIn plain words
(R10)⊤R10=I(\mathbf{R}_1^0)^\top\mathbf{R}_1^0 = \mathbf{I}the columns are unit vectors at right angles to each other
(R10)−1=(R10)⊤=R01(\mathbf{R}_1^0)^{-1} = (\mathbf{R}_1^0)^\top = \mathbf{R}_0^1going back is a transpose
det⁡(R10)=+1\det(\mathbf{R}_1^0) = +1it only turns things — no stretching and no mirror image
p^0 = R_1^0 p^1      = [1 1]
R_0^1 =
 [[ 0  1]
 [-1  0]]
p^1 = R_0^1 p^0      = [ 1 -1]

R^T R = I ? True     det(R) = 1.0
R_1^0 equals rot2(90°)? True

4.2.4 Combining two rotations in 2D

If frame {2}\{2\} is turned by β\beta relative to frame {1}\{1\}, and frame {1}\{1\} is turned by α\alpha relative to frame {0}\{0\}, the indices chain just like in Section 4.1:

R20=R10 R21=R(α) R(β)=R(α+β).\mathbf{R}_2^0 = \mathbf{R}_1^0\,\mathbf{R}_2^1 = \mathbf{R}(\alpha)\,\mathbf{R}(\beta) = \mathbf{R}(\alpha+\beta).

In 2D the angles simply add, so the order does not matter: R(α)R(β)=R(β)R(α)\mathbf{R}(\alpha)\mathbf{R}(\beta) = \mathbf{R}(\beta)\mathbf{R}(\alpha). For the 2R arm this is the familiar fact that link 2 points at the angle θ1+θ2\theta_1+\theta_2.

Careful: this “order does not matter” is special to 2D. In 3D the order of rotations does matter (Section 4.3.2).

Loading...

4.2.5 Position + orientation together: the 2D HTM

Now frame {1}\{1\} is both moved (origin at o10\mathbf{o}_1^0) and turned (R10\mathbf{R}_1^0). A point qq has coordinates q1\mathbf{q}^1 in frame {1}\{1\}. Where is it in frame {0}\{0\}? Think of it as two moves (Lecture 6):

  1. Turn the arrow from o1o_1 to qq into frame-{0}\{0\} directions: R10 q1\mathbf{R}_1^0\,\mathbf{q}^1.

  2. Add where that arrow starts, o1o_1: + o10+\ \mathbf{o}_1^0.

q0=R10 q1+o10\boxed{\mathbf{q}^0 = \mathbf{R}_1^0\,\mathbf{q}^1 + \mathbf{o}_1^0}

Figure 4.5 — A point q seen from a moved and turned frame: q0 = R10 q1 + o10.

That’s correct, but it is a multiply and an add. With many frames in a chain we’d alternate multiply, add, multiply, add… There’s a neat trick to fold both into one multiplication: put a 1 under each point and pack R10\mathbf{R}_1^0 and o10\mathbf{o}_1^0 into one bigger matrix:

[q01]⏟q~0=[R10o100  01]⏟T10 (3×3)[q11]⏟q~1,written out:[qx0qy01]=[r11r12oxr21r22oy001][qx1qy11].\underbrace{\begin{bmatrix}\mathbf{q}^0\\1\end{bmatrix}}_{\tilde{\mathbf{q}}^0} = \underbrace{\begin{bmatrix}\mathbf{R}_1^0 & \mathbf{o}_1^0\\ 0\ \ 0 & 1\end{bmatrix}}_{\mathbf{T}_1^0\ (3\times3)} \underbrace{\begin{bmatrix}\mathbf{q}^1\\1\end{bmatrix}}_{\tilde{\mathbf{q}}^1}, \qquad\text{written out:}\qquad \begin{bmatrix}q_x^0\\ q_y^0\\ 1\end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & o_x \\ r_{21} & r_{22} & o_y \\ 0 & 0 & 1\end{bmatrix} \begin{bmatrix}q_x^1\\ q_y^1\\ 1\end{bmatrix}.

Multiply the first two rows out and you get exactly q0=R10q1+o10\mathbf{q}^0 = \mathbf{R}_1^0\mathbf{q}^1 + \mathbf{o}_1^0; the third row just says 1=11 = 1. The extra row and column are bookkeeping, not new physics. T10\mathbf{T}_1^0 is the homogeneous transformation matrix: the posture (position and orientation) of frame {1}\{1\} relative to frame {0}\{0\}.

q~0=T10 q~1and, going back,q~1=T01 q~0,T01=(T10)−1.\tilde{\mathbf{q}}^0 = \mathbf{T}_1^0\,\tilde{\mathbf{q}}^1 \qquad\text{and, going back,}\qquad \tilde{\mathbf{q}}^1 = \mathbf{T}_0^1\,\tilde{\mathbf{q}}^0,\quad \mathbf{T}_0^1 = \left(\mathbf{T}_1^0\right)^{-1}.

Worked example. Frame {1}\{1\} has its origin at o10=[1, 1]⊤\mathbf{o}_1^0 = [1,\ 1]^\top and is turned by 90∘90^\circ (x1x_1 along y0y_0, y1y_1 along −x0-x_0). A point is 1 unit along x1x_1: q1=[1, 0]⊤\mathbf{q}^1 = [1,\ 0]^\top. Then

q~0=T10 q~1=[0−11101001][101]=[0⋅1+(−1)⋅0+1⋅11⋅1+0⋅0+1⋅10+0+1]=[121] ⇒ q0=[12].\tilde{\mathbf{q}}^0 = \mathbf{T}_1^0\,\tilde{\mathbf{q}}^1 = \begin{bmatrix}0 & -1 & 1\\ 1 & 0 & 1\\ 0 & 0 & 1\end{bmatrix} \begin{bmatrix}1\\0\\1\end{bmatrix} = \begin{bmatrix}0\cdot1 + (-1)\cdot0 + 1\cdot1\\ 1\cdot1 + 0\cdot0 + 1\cdot1\\ 0+0+1\end{bmatrix} = \begin{bmatrix}1\\2\\1\end{bmatrix} \ \Rightarrow\ \mathbf{q}^0 = \begin{bmatrix}1\\2\end{bmatrix}.

Sanity check with a sketch: start at o1=(1,1)o_1 = (1,1) and walk 1 unit along x1x_1, which points “up” (+y0+y_0). You land at (1,2)(1, 2). ✓

T_1^0 =
 [[ 0. -1.  1.]
 [ 1.  0.  1.]
 [ 0.  0.  1.]]

R q1 + o          : [1. 2.]
T_1^0 [q1; 1]     : [1. 2. 1.]  -> q^0 = [1. 2.]

Back again, T_0^1 [q0; 1] : [1. 0. 1.]  -> q^1 = [1. 0.]

4.2.6 Example: forward kinematics of the 2R arm with HTMs

Let’s redo the Chapter 3 problem with HTMs (Lecture 6). Frames as in the widget of Section 4.1: frame {1}\{1\} at the elbow with x1x_1 along link 1, frame {2}\{2\} at the tip with x2x_2 along link 2. Shorthand: c1=cos⁡θ1c_1 = \cos\theta_1, s1=sin⁡θ1s_1 = \sin\theta_1, and so on.

Figure 4.6 — The 2R arm with frame {1} at the elbow and frame {2} at the tip (as in the lecture notes).

Step 1 — frame {1}\{1\} relative to frame {0}\{0\}. x1x_1 is turned by θ1\theta_1, and o1o_1 is at the end of link 1:

T10=[R10o100  01]=[c1−s1L1c1s1c1L1s1001].\mathbf{T}_1^0 = \begin{bmatrix}\mathbf{R}_1^0 & \mathbf{o}_1^0\\ 0\ \ 0 & 1\end{bmatrix} = \begin{bmatrix} c_1 & -s_1 & L_1c_1\\ s_1 & c_1 & L_1s_1\\ 0 & 0 & 1\end{bmatrix}.

Step 2 — frame {2}\{2\} relative to frame {1}\{1\}. Seen from frame {1}\{1\}, link 2 is turned by θ2\theta_2 and has length L2L_2 — exactly the same form:

T21=[R21o210  01]=[c2−s2L2c2s2c2L2s2001].\mathbf{T}_2^1 = \begin{bmatrix}\mathbf{R}_2^1 & \mathbf{o}_2^1\\ 0\ \ 0 & 1\end{bmatrix} = \begin{bmatrix} c_2 & -s_2 & L_2c_2\\ s_2 & c_2 & L_2s_2\\ 0 & 0 & 1\end{bmatrix}.

Step 3 — chain them. The tip is the origin o2o_2. In its own frame it is o22=[0, 0]⊤\mathbf{o}_2^2 = [0,\ 0]^\top, so

o~20=T10 T21 o~22=T10 T21[001]=T10[L2c2L2s21]=[L2c1c2−L2s1s2+L1c1L2s1c2+L2c1s2+L1s11].\tilde{\mathbf{o}}_2^0 = \mathbf{T}_1^0\,\mathbf{T}_2^1\,\tilde{\mathbf{o}}_2^2 = \mathbf{T}_1^0\,\mathbf{T}_2^1\begin{bmatrix}0\\0\\1\end{bmatrix} = \mathbf{T}_1^0\begin{bmatrix}L_2c_2\\ L_2s_2\\ 1\end{bmatrix} = \begin{bmatrix}L_2c_1c_2 - L_2s_1s_2 + L_1c_1\\ L_2s_1c_2 + L_2c_1s_2 + L_1s_1\\ 1\end{bmatrix}.

Using cos⁡(θ1+θ2)=c1c2−s1s2\cos(\theta_1+\theta_2) = c_1c_2 - s_1s_2 and sin⁡(θ1+θ2)=s1c2+c1s2\sin(\theta_1+\theta_2) = s_1c_2 + c_1s_2:

o20=[L1cos⁡θ1+L2cos⁡(θ1+θ2)L1sin⁡θ1+L2sin⁡(θ1+θ2)]\mathbf{o}_2^0 = \begin{bmatrix}L_1\cos\theta_1 + L_2\cos(\theta_1+\theta_2)\\ L_1\sin\theta_1 + L_2\sin(\theta_1+\theta_2)\end{bmatrix}

— exactly the Chapter 3 answer, but now produced by a recipe (write one HTM per link, multiply) instead of by staring at triangles. The product T20=T10T21\mathbf{T}_2^0 = \mathbf{T}_1^0\mathbf{T}_2^1 also gives the tip’s orientation for free: its rotation block is R(θ1+θ2)\mathbf{R}(\theta_1+\theta_2).

Chapter 5 places the frames slightly differently (frame {i}\{i\} sits on joint ii, with ziz_i along the joint axis, and an extra frame {e}\{e\} at the gripper). That choice makes the recipe work for any 3D robot. The tip position comes out the same.

The cell below lets SymPy do the algebra of Step 3 for us.

Loading...
Loading...
Loading...

Key Takeaways — Section 4.2

  • Posture = position o10\mathbf{o}_1^0 + orientation R10\mathbf{R}_1^0.

  • R10=[x10  y10]\mathbf{R}_1^0 = [\mathbf{x}_1^0\ \ \mathbf{y}_1^0]: the columns are frame {1}\{1\}'s axes written in frame {0}\{0\}. Each entry is a dot product of two axes.

  • A rotation matrix converts coordinates: p0=R10p1\mathbf{p}^0 = \mathbf{R}_1^0\mathbf{p}^1; to go back, transpose: R01=(R10)⊤\mathbf{R}_0^1 = (\mathbf{R}_1^0)^\top.

  • Moved and turned frame: q0=R10q1+o10\mathbf{q}^0 = \mathbf{R}_1^0\mathbf{q}^1 + \mathbf{o}_1^0, or in one step q~0=T10q~1\tilde{\mathbf{q}}^0 = \mathbf{T}_1^0\tilde{\mathbf{q}}^1 with T10=[R10o100⊤1]\mathbf{T}_1^0 = \begin{bmatrix}\mathbf{R}_1^0 & \mathbf{o}_1^0\\ \mathbf{0}^\top & 1\end{bmatrix}.

  • Chaining frames is matrix multiplication: T20=T10T21\mathbf{T}_2^0 = \mathbf{T}_1^0\mathbf{T}_2^1. For the 2R arm this reproduces Chapter 3’s formula.

4.3 Orientation in 3D

4.3.1 The 3D rotation matrix

Everything from 2D carries over — we just add the zz axis (Lecture 7). The orientation of frame {1}\{1\} relative to frame {0}\{0\} is again “frame 1’s axes, written in frame 0”:

R10=[x10y10z10]=[x1⋅x0y1⋅x0z1⋅x0x1⋅y0y1⋅y0z1⋅y0x1⋅z0y1⋅z0z1⋅z0]3×3\mathbf{R}_1^0 = \begin{bmatrix}\mathbf{x}_1^0 & \mathbf{y}_1^0 & \mathbf{z}_1^0\end{bmatrix} = \begin{bmatrix} x_1\cdot x_0 & y_1\cdot x_0 & z_1\cdot x_0\\ x_1\cdot y_0 & y_1\cdot y_0 & z_1\cdot y_0\\ x_1\cdot z_0 & y_1\cdot z_0 & z_1\cdot z_0 \end{bmatrix}_{3\times3}

For simple drawings where every axis of frame {1}\{1\} lines up with some axis of frame {0}\{0\}, each column is just one ±1\pm1 and two zeros: ask “which way does x1x_1 point, in frame-0 terms?”

Example 1. x1x_1 points along z0z_0, y1y_1 points along x0x_0, z1z_1 points along y0y_0:

R10=[x10y10z10]=[010001100],R01=(R10)⊤=[001100010].\mathbf{R}_1^0 = \begin{bmatrix}\mathbf{x}_1^0 & \mathbf{y}_1^0 & \mathbf{z}_1^0\end{bmatrix} = \begin{bmatrix}0 & 1 & 0\\ 0 & 0 & 1\\ 1 & 0 & 0\end{bmatrix}, \qquad \mathbf{R}_0^1 = \left(\mathbf{R}_1^0\right)^\top = \begin{bmatrix}0 & 0 & 1\\ 1 & 0 & 0\\ 0 & 1 & 0\end{bmatrix}.

Figure 4.7 — Example 1: every axis of frame {1} lines up with an axis of frame {0}.

Example 2 (tilted axes). z0z_0, x0x_0, x1x_1 and y1y_1 all lie in one plane; x1x_1 is 30∘30^\circ from z0z_0, and z1z_1 points along y0y_0. Use the dot products — for unit vectors a⋅b=cos⁡(angle between them)a\cdot b = \cos(\text{angle between them}):

R10=[sin⁡30∘cos⁡30∘0001cos⁡30∘−sin⁡30∘0].\mathbf{R}_1^0 = \begin{bmatrix} \sin30^\circ & \cos30^\circ & 0\\ 0 & 0 & 1\\ \cos30^\circ & -\sin30^\circ & 0 \end{bmatrix}.

Figure 4.8 — Example 2: x1 is tilted 30° from z0; z1 points along y0.

Right-hand rule check. In a right-handed frame z=x×yz = x\times y. You can use this to fill in a column you can’t see in the drawing, or to catch a mistake: if x10×y10≠z10\mathbf{x}_1^0\times\mathbf{y}_1^0 \neq \mathbf{z}_1^0 (equivalently det⁡R≠+1\det\mathbf{R}\neq+1), something is wrong.

<Figure size 1200x500 with 2 Axes>
Example 1:  x1 × y1 = z1 ? True   det = 1.000   R^T R = I ? True
Example 2:  x1 × y1 = z1 ? True   det = 1.000   R^T R = I ? True

Example 1:  R_0^1 = (R_1^0)^T =
 [[0. 0. 1.]
 [1. 0. 0.]
 [0. 1. 0.]]

Converting coordinates in 3D

Exactly as in 2D:

q0=R10 q1,q1=R01 q0=(R10)⊤q0.\mathbf{q}^0 = \mathbf{R}_1^0\,\mathbf{q}^1, \qquad \mathbf{q}^1 = \mathbf{R}_0^1\,\mathbf{q}^0 = \left(\mathbf{R}_1^0\right)^\top\mathbf{q}^0.

Example (Lecture 7). Use the frames of Example 1. A point qq lies in the y0z0y_0z_0 plane, 1 unit along y0y_0 and 1 unit along z0z_0, so q0=[0, 1, 1]⊤\mathbf{q}^0 = [0,\ 1,\ 1]^\top. Measured along frame {1}\{1\}'s axes it is 1 along x1x_1 (which points along z0z_0) and 1 along z1z_1 (which points along y0y_0), so q1=[1, 0, 1]⊤\mathbf{q}^1 = [1,\ 0,\ 1]^\top. Check:

R10 q1=[010001100][101]=[0⋅1+1⋅0+0⋅10⋅1+0⋅0+1⋅11⋅1+0⋅0+0⋅1]=[011]=q0.✓\mathbf{R}_1^0\,\mathbf{q}^1 = \begin{bmatrix}0 & 1 & 0\\ 0 & 0 & 1\\ 1 & 0 & 0\end{bmatrix} \begin{bmatrix}1\\0\\1\end{bmatrix} = \begin{bmatrix}0\cdot1 + 1\cdot0 + 0\cdot1\\ 0\cdot1 + 0\cdot0 + 1\cdot1\\ 1\cdot1 + 0\cdot0 + 0\cdot1\end{bmatrix} = \begin{bmatrix}0\\1\\1\end{bmatrix} = \mathbf{q}^0. \checkmark

Figure 4.9 — Lecture 7 example: point q in the y0z0 plane, using the frames of Example 1.

q^0 = R_1^0 q^1 = [0. 1. 1.]
q^1 = R_0^1 q^0 = [1. 0. 1.]   (back where we started)

The three basic rotation matrices

A rotation matrix can also describe a motion: start with frame {1}\{1\} on top of frame {0}\{0\}, then turn it by θ\theta about one of frame {0}\{0\}'s axes. Reading the new axes column by column gives three special cases (Lecture 7):

Rz(θ)=[cos⁡θ−sin⁡θ0sin⁡θcos⁡θ0001],Rx(θ)=[1000cos⁡θ−sin⁡θ0sin⁡θcos⁡θ],Ry(θ)=[cos⁡θ0sin⁡θ010−sin⁡θ0cos⁡θ]\mathbf{R}_z(\theta) = \begin{bmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix},\quad \mathbf{R}_x(\theta) = \begin{bmatrix}1&0&0\\0&\cos\theta&-\sin\theta\\0&\sin\theta&\cos\theta\end{bmatrix},\quad \mathbf{R}_y(\theta) = \begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix}

Figure 4.10 — The three basic rotations: frame {1} turned by θ about z0, x0, or y0.

Ways to remember them:

  • The axis you rotate about doesn’t move, so its row and column are a 1 and zeros.

  • The other 2×22\times2 block is the 2D rotation matrix.

  • Ry\mathbf{R}_y looks “flipped” (the minus sign is at the bottom left). That is not a typo: about yy, the right-hand rule turns zz toward xx (the order is z→xz \to x, not x→zx \to z), so a positive θ\theta tips z1z_1 toward +x0+x_0.

In Chapter 5 these become the rotation blocks of the basic HTMs Rotx,α\mathrm{Rot}_{x,\alpha}, Roty,β\mathrm{Rot}_{y,\beta}, Rotz,γ\mathrm{Rot}_{z,\gamma}.

Loading...

4.3.2 Combining Rotations: Order Matters, and Fixed vs. Current Axes

In 3D, order matters. Take a book. Turn it 90∘90^\circ about xx, then 90∘90^\circ about yy. Now start again and do yy first, then xx. The book ends up in two different poses. So unlike 2D, we can’t just add up angles; we have to multiply matrices in the right order.

<Figure size 1200x500 with 2 Axes>
R_y(90°) R_x(90°) =
 [[ 0.  1.  0.]
 [ 0.  0. -1.]
 [-1.  0.  0.]]

R_x(90°) R_y(90°) =
 [[ 0.  0.  1.]
 [ 1.  0. -0.]
 [-0.  1.  0.]]

Equal? False  <- in 3D, order matters!

Which axis does a new rotation act about? It depends on which side you multiply on.

You writeThe new rotation Radd\mathbf{R}_\text{add} turns the frame about …Name
Rcurrent Radd\mathbf{R}_\text{current}\,\mathbf{R}_\text{add}the frame’s current (own, “body”) axespost-multiply (on the right)
Radd Rcurrent\mathbf{R}_\text{add}\,\mathbf{R}_\text{current}the fixed axes of frame {0}\{0\}pre-multiply (on the left)

Why the right side means “current axes”. Write it with frames: R20=R10 R21\mathbf{R}_2^0 = \mathbf{R}_1^0\,\mathbf{R}_2^1. The extra rotation R21\mathbf{R}_2^1 describes frame {2}\{2\} relative to frame {1}\{1\} — so it is measured about frame {1}\{1\}'s own axes, wherever they currently point. This is the rule we use all the time in Chapter 5: a product like Rz(θ1) Rx(α)⋯\mathbf{R}_z(\theta_1)\,\mathbf{R}_x(\alpha)\cdots is read left to right, each turn about the frame’s current axes.

Picture it: a drone. “Pitch up relative to where my nose points” (what the pilot does) is post-multiplication. “Tilt about the room’s fixed yy axis, whichever way I’m facing” is pre-multiplication.

Figure 4.11 — Same second turn, different axis: about the current x1 (multiply on the right) vs. the fixed x0 (multiply on the left).

The widget starts from the pose Rcurrent=Ry(45∘)Rx(30∘)\mathbf{R}_\text{current} = \mathbf{R}_y(45^\circ)\mathbf{R}_x(30^\circ) and adds Rz(θ)\mathbf{R}_z(\theta) on either side. Watch which blue zz axis the frame spins about: its own (bold), or the faded fixed z0z_0.

Loading...

4.3.3 Other Ways to Describe 3D Orientation

A 3×33\times3 rotation matrix has 9 numbers, but only 3 of them are independent — an orientation has 3 degrees of freedom (think yaw, pitch, roll). Robotics software uses several other, more compact descriptions. You will meet all of them in libraries like MuJoCo, ROS and SciPy:

Representation# numbersGood forWatch out for
Rotation matrix9computing, chaining frames6 of the 9 numbers are redundant
Axis-angle4 (axis + angle)intuition: “one spin about one axis”the axis is undefined when the angle is 0
Euler angles3humans: yaw / pitch / rollgimbal lock; 12 different conventions
Quaternion4smooth interpolation, simulatorsless intuitive

Rule of thumb: compute with rotation matrices, show Euler angles to humans, let the software use quaternions.

Axis-angle and Rodrigues’ formula

Idea: any 3D orientation can be reached with a single turn by some angle θ\theta about some unit axis k^\hat{\mathbf{k}} — like turning a screwdriver whose shaft points along k^\hat{\mathbf{k}}. Rodrigues’ formula turns (k^,θ)(\hat{\mathbf{k}},\theta) into a rotation matrix:

R=I+sin⁡θ [k^]×+(1−cos⁡θ) [k^]×2,[k^]×=[0−kzkykz0−kx−kykx0].\mathbf{R} = \mathbf{I} + \sin\theta\,[\hat{\mathbf{k}}]_\times + (1-\cos\theta)\,[\hat{\mathbf{k}}]_\times^2, \qquad [\hat{\mathbf{k}}]_\times = \begin{bmatrix}0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0\end{bmatrix}.

Figure 4.12 — Axis-angle: one turn by θ about a unit axis k̂.

[k^]×[\hat{\mathbf{k}}]_\times is just the cross product written as a matrix: [k^]×v=k^×v[\hat{\mathbf{k}}]_\times\mathbf{v} = \hat{\mathbf{k}}\times\mathbf{v}. You don’t need to memorize the formula; the useful facts are:

  • θ=0\theta = 0 gives R=I\mathbf{R} = \mathbf{I} (no turn).

  • k^=(0,0,1)\hat{\mathbf{k}} = (0,0,1) gives exactly Rz(θ)\mathbf{R}_z(\theta) — the basic rotations are special cases (try it in the widget: set k=(0,0,1)k = (0,0,1)).

  • Libraries such as MuJoCo and ROS use this formula internally.

Rodrigues with k = z-axis equals R_z(θ)? True
Loading...

Euler angles — ZYX (yaw, pitch, roll)

Euler angles build an orientation from three turns in a row about coordinate axes. The most common convention in robotics and aviation is ZYX, also called yaw–pitch–roll:

R=Rz(ψ) Ry(θ) Rx(ϕ)\mathbf{R} = \mathbf{R}_z(\psi)\,\mathbf{R}_y(\theta)\,\mathbf{R}_x(\phi)
AngleAxisNameThink of …
ψ\psizzyawcompass heading
θ\thetayypitchnose up / down
ϕ\phixxrolltilting the wings

Figure 4.13 — Yaw ψ about z, pitch θ about y, roll φ about x.

Read the product left to right with the rule from Section 4.3.2: first yaw about zz, then pitch about the new yy, then roll about the newest xx — each turn about the current axes.

Gimbal lock. When the pitch is θ=±90∘\theta = \pm90^\circ, the yaw axis and the roll axis line up, so yaw and roll do the same thing and one degree of freedom is lost. Try it in the widget: set pitch to 90∘90^\circ and move yaw and roll — they produce the same kind of motion.

You’ll see another convention in Chapter 5: a spherical wrist produces Rz(θ1)Ry(θ2)Rz(θ3)\mathbf{R}_z(\theta_1)\mathbf{R}_y(\theta_2)\mathbf{R}_z(\theta_3), the ZYZ Euler angles.

Loading...

Quaternions

A unit quaternion stores an orientation in four numbers: a scalar part ww and a 3-vector part v=(vx,vy,vz)\mathbf{v} = (v_x, v_y, v_z). It is built straight from the axis-angle pair (k^,θ)(\hat{\mathbf{k}}, \theta):

Q=(cos⁡θ2⏟w,  sin⁡θ2 k^⏟v),w2+vx2+vy2+vz2=1.\mathcal{Q} = \Bigl(\underbrace{\cos\tfrac{\theta}{2}}_{w},\ \ \underbrace{\sin\tfrac{\theta}{2}\,\hat{\mathbf{k}}}_{\mathbf{v}}\Bigr), \qquad w^2 + v_x^2 + v_y^2 + v_z^2 = 1.

(We write quaternions as Q\mathcal{Q} so they don’t get mixed up with points q\mathbf{q}.) So a quaternion is really axis-angle, repackaged so that combining rotations becomes a simple multiplication.

Why bother?

  1. No gimbal lock — every orientation is handled smoothly.

  2. Smooth in-between orientations — SLERP turns at a constant rate from one orientation to another (great for animation and trajectories).

  3. Cheap and numerically stable — that’s why simulators use them.

Watch the order of the four numbers. MuJoCo stores quaternions scalar-first, (w,vx,vy,vz)(w, v_x, v_y, v_z) — e.g. data.xquat, and qpos of free/ball joints. SciPy’s Rotation uses scalar-last, (vx,vy,vz,w)(v_x, v_y, v_z, w). Mixing these up is one of the most common bugs in robotics code.

In practice you let a library (scipy.spatial.transform.Rotation, or MuJoCo’s mju_* functions) convert between representations. But it’s worth doing the two core operations by hand once, so the library isn’t a black box.

Converting and combining quaternions by hand

1. Quaternion → rotation matrix. For Q=(w,vx,vy,vz)\mathcal{Q} = (w, v_x, v_y, v_z):

R(Q)=[1−2(vy2+vz2)2(vxvy−wvz)2(vxvz+wvy)2(vxvy+wvz)1−2(vx2+vz2)2(vyvz−wvx)2(vxvz−wvy)2(vyvz+wvx)1−2(vx2+vy2)]\mathbf{R}(\mathcal{Q}) = \begin{bmatrix} 1-2(v_y^2+v_z^2) & 2(v_xv_y-wv_z) & 2(v_xv_z+wv_y)\\ 2(v_xv_y+wv_z) & 1-2(v_x^2+v_z^2) & 2(v_yv_z-wv_x)\\ 2(v_xv_z-wv_y) & 2(v_yv_z+wv_x) & 1-2(v_x^2+v_y^2) \end{bmatrix}

2. Combining two rotations (the Hamilton product). The quaternion version of the matrix product RaRb\mathbf{R}_a\mathbf{R}_b is Qa⊗Qb\mathcal{Q}_a\otimes\mathcal{Q}_b (same order):

Qa⊗Qb=(wawb−va⋅vb⏟scalar part,  wavb+wbva+va×vb⏟vector part)\mathcal{Q}_a\otimes\mathcal{Q}_b = \Bigl(\underbrace{w_aw_b - \mathbf{v}_a\cdot\mathbf{v}_b}_{\text{scalar part}},\ \ \underbrace{w_a\mathbf{v}_b + w_b\mathbf{v}_a + \mathbf{v}_a\times\mathbf{v}_b}_{\text{vector part}}\Bigr)

It looks long, but it’s just a dot product for the scalar part and “weighted sum plus a cross product” for the vector part.

Bonus — the inverse is free (just like R−1=R⊤\mathbf{R}^{-1} = \mathbf{R}^\top): for a unit quaternion, flip the sign of the vector part, Q−1=(w,−v)\mathcal{Q}^{-1} = (w, -\mathbf{v}).

The cells below use exactly these formulas on numbers small enough to check on paper.

=== Quaternion from axis-angle: 90° about z ===

Q = (cos45°, sin45°·k̂) = (0.7071, 0.0000, 0.0000, 0.7071)
|Q|² = 1.000000   (unit quaternion ✓)

SciPy as_quat() (scalar-last)  : [0.     0.     0.7071 0.7071]
reordered to (w, vx, vy, vz)    : [0.7071 0.     0.     0.7071]

SciPy rotation matrix equals R_z(90°)? True

=== SLERP from 0° to 90° about z ===
  t = 0.00  ->  angle about z =   0.0°
  t = 0.25  ->  angle about z =  22.5°
  t = 0.50  ->  angle about z =  45.0°
  t = 0.75  ->  angle about z =  67.5°
  t = 1.00  ->  angle about z =  90.0°

Example 1 — Quaternions for the planar 2R arm

Every joint of the 2R arm turns about the same fixed axis, z0z_0. So the orientation of each link frame is a single turn about k^=(0,0,1)\hat{\mathbf{k}} = (0,0,1) and its quaternion is simply

Q=(cos⁡θ2, 0, 0, sin⁡θ2).\mathcal{Q} = \Bigl(\cos\tfrac{\theta}{2},\ 0,\ 0,\ \sin\tfrac{\theta}{2}\Bigr).

Frame {1}\{1\} is turned by θ1\theta_1 and frame {2}\{2\} by θ1+θ2\theta_1+\theta_2 (Section 4.2.4). With θ1=45∘\theta_1 = 45^\circ, θ2=60∘\theta_2 = 60^\circ:

Q10=(cos⁡22.5∘, 0, 0, sin⁡22.5∘)≈(0.9239, 0, 0, 0.3827),Q20=(cos⁡52.5∘, 0, 0, sin⁡52.5∘)≈(0.6088, 0, 0, 0.7934).\mathcal{Q}_1^0 = (\cos22.5^\circ,\,0,\,0,\,\sin22.5^\circ) \approx (0.9239,\ 0,\ 0,\ 0.3827), \qquad \mathcal{Q}_2^0 = (\cos52.5^\circ,\,0,\,0,\,\sin52.5^\circ) \approx (0.6088,\ 0,\ 0,\ 0.7934).

The cell below checks Q20\mathcal{Q}_2^0 against Rz(θ1+θ2)\mathbf{R}_z(\theta_1+\theta_2) using the conversion formula, and — if mujoco is installed — against MuJoCo’s own data.xquat for a 2R model like the one in Chapter 3. They agree up to an overall sign: Q\mathcal{Q} and −Q-\mathcal{Q} describe the same orientation, so a library may return either.

Q_1^0 = [0.9239 0.     0.     0.3827]
Q_2^0 = [0.6088 0.     0.     0.7934]

quat_to_matrix(Q_2^0) == R_z(θ1+θ2)? True

(mujoco not installed — skipping the MuJoCo cross-check.)

Example 2 — Combining two 90° turns by hand

In Section 4.3.2 we let NumPy compute Ry(90∘) Rx(90∘)\mathbf{R}_y(90^\circ)\,\mathbf{R}_x(90^\circ). Let’s get the same orientation with quaternions.

For a 90∘90^\circ turn, θ/2=45∘\theta/2 = 45^\circ and cos⁡45∘=sin⁡45∘=c=22\cos45^\circ = \sin45^\circ = c = \tfrac{\sqrt2}{2}:

Qx=(c, c, 0, 0),Qy=(c, 0, c, 0).\mathcal{Q}_x = (c,\ c,\ 0,\ 0), \qquad \mathcal{Q}_y = (c,\ 0,\ c,\ 0).

Ry(90∘)Rx(90∘)\mathbf{R}_y(90^\circ)\mathbf{R}_x(90^\circ) corresponds to Qy⊗Qx\mathcal{Q}_y\otimes\mathcal{Q}_x. With vy=(0,c,0)\mathbf{v}_y = (0,c,0), vx=(c,0,0)\mathbf{v}_x = (c,0,0) and c2=12c^2 = \tfrac12:

  • scalar part: c⋅c−vy⋅vx=12−0=12c\cdot c - \mathbf{v}_y\cdot\mathbf{v}_x = \tfrac12 - 0 = \tfrac12

  • vector part: c vx+c vy+vy×vx=(12,0,0)+(0,12,0)+(0,0,−12)c\,\mathbf{v}_x + c\,\mathbf{v}_y + \mathbf{v}_y\times\mathbf{v}_x = (\tfrac12, 0, 0) + (0, \tfrac12, 0) + (0, 0, -\tfrac12)

Qy⊗Qx=(12, 12, 12, −12).\mathcal{Q}_y\otimes\mathcal{Q}_x = \bigl(\tfrac12,\ \tfrac12,\ \tfrac12,\ -\tfrac12\bigr).

No calculator needed. The cell checks it against the matrix product.

Q_y ⊗ Q_x = [ 0.5  0.5  0.5 -0.5]   (hand result: 0.5, 0.5, 0.5, -0.5)

As a rotation matrix:
 [[-0.  1.  0.]
 [ 0. -0. -1.]
 [-1.  0. -0.]]

R_y(90°) R_x(90°):
 [[ 0.  1.  0.]
 [ 0.  0. -1.]
 [-1.  0.  0.]]

Match? True

Key Takeaways — Section 4.3

  • R10=[x10  y10  z10]\mathbf{R}_1^0 = [\mathbf{x}_1^0\ \ \mathbf{y}_1^0\ \ \mathbf{z}_1^0]; each entry is a dot product of an axis of {1}\{1\} with an axis of {0}\{0\}. Check with z=x×y\mathbf{z} = \mathbf{x}\times\mathbf{y}.

  • Convert coordinates with q0=R10q1\mathbf{q}^0 = \mathbf{R}_1^0\mathbf{q}^1; go back with the transpose, R01=(R10)⊤\mathbf{R}_0^1 = (\mathbf{R}_1^0)^\top.

  • Three basic rotations Rx(θ)\mathbf{R}_x(\theta), Ry(θ)\mathbf{R}_y(\theta), Rz(θ)\mathbf{R}_z(\theta) (watch the sign pattern of Ry\mathbf{R}_y).

  • In 3D the order of rotations matters. Multiplying on the right (RcurrentRadd\mathbf{R}_\text{current}\mathbf{R}_\text{add}) turns about the frame’s current axes; on the left, about the fixed axes. Products are read left to right with current axes — the rule used in Chapter 5.

  • Axis-angle, Euler angles (ZYX = yaw, pitch, roll; gimbal lock at pitch ±90∘\pm90^\circ) and quaternions describe the same orientations more compactly.

  • Quaternions: combine with the Hamilton product (same order as matrices), invert by flipping the vector part. MuJoCo is scalar-first (w,vx,vy,vz)(w, v_x, v_y, v_z); SciPy is scalar-last. Q\mathcal{Q} and −Q-\mathcal{Q} are the same orientation.

4.4 The Homogeneous Transformation Matrix in 3D

4.4.1 Structure

Same idea as Section 4.2.5, one dimension up (Lecture 7). If frame {1}\{1\} has origin o10\mathbf{o}_1^0 and orientation R10\mathbf{R}_1^0, a point with coordinates q1\mathbf{q}^1 in frame {1}\{1\} has coordinates

q0=R10 q1+o10\mathbf{q}^0 = \mathbf{R}_1^0\,\mathbf{q}^1 + \mathbf{o}_1^0

in frame {0}\{0\}. Put a 1 under the points and pack everything into one 4×44\times4 matrix:

[q01]⏟q~0=[R10o100  0  01]⏟T10[q11]⏟q~1,[qx0qy0qz01]=[r11r12r13oxr21r22r23oyr31r32r33oz0001][qx1qy1qz11].\underbrace{\begin{bmatrix}\mathbf{q}^0\\1\end{bmatrix}}_{\tilde{\mathbf{q}}^0} = \underbrace{\begin{bmatrix}\mathbf{R}_1^0 & \mathbf{o}_1^0\\ 0\ \ 0\ \ 0 & 1\end{bmatrix}}_{\mathbf{T}_1^0} \underbrace{\begin{bmatrix}\mathbf{q}^1\\1\end{bmatrix}}_{\tilde{\mathbf{q}}^1}, \qquad \begin{bmatrix}q_x^0\\ q_y^0\\ q_z^0\\ 1\end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & o_x\\ r_{21} & r_{22} & r_{23} & o_y\\ r_{31} & r_{32} & r_{33} & o_z\\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}q_x^1\\ q_y^1\\ q_z^1\\ 1\end{bmatrix}.

Figure 4.14 — The 3D HTM moves a point from frame {1} coordinates to frame {0} coordinates.

Reading an HTM column by column — you can see the whole frame in it:

ColumnWhat it is
1x10\mathbf{x}_1^0: where x1x_1 points, in frame {0}\{0\}
2y10\mathbf{y}_1^0: where y1y_1 points
3z10\mathbf{z}_1^0: where z1z_1 points
4o10\mathbf{o}_1^0: where the origin o1o_1 is
bottom rowalways [0  0  0  1][0\ \ 0\ \ 0\ \ 1]

(Chapter 5 calls these four columns n,s,a,d\mathbf{n}, \mathbf{s}, \mathbf{a}, \mathbf{d}.)

Figure 4.15 — Lecture 7 example: frame {1} at (0, 1, 0), turned 90° about z0.

Worked example (Lecture 7). Frame {1}\{1\}'s origin is 1 unit along y0y_0: o10=[0, 1, 0]⊤\mathbf{o}_1^0 = [0,\ 1,\ 0]^\top. Its axes: x1x_1 along y0y_0, y1y_1 along −x0-x_0, z1z_1 along z0z_0 (a 90∘90^\circ turn about z0z_0). A point sits at q0=[1, 1, 0]⊤\mathbf{q}^0 = [1,\ 1,\ 0]^\top. Its coordinates in frame {1}\{1\} are q1=[0, −1, 0]⊤\mathbf{q}^1 = [0,\ -1,\ 0]^\top (it is 1 unit in the −y1-y_1 direction from o1o_1). Check:

q~0=T10 q~1=[0−100100100100001][0−101]=[1101].✓\tilde{\mathbf{q}}^0 = \mathbf{T}_1^0\,\tilde{\mathbf{q}}^1 = \begin{bmatrix}0 & -1 & 0 & 0\\ 1 & 0 & 0 & 1\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}0\\-1\\0\\1\end{bmatrix} = \begin{bmatrix}1\\1\\0\\1\end{bmatrix}. \checkmark
T_1^0 =
 [[ 0. -1.  0.  0.]
 [ 1.  0.  0.  1.]
 [ 0.  0.  1.  0.]
 [ 0.  0.  0.  1.]]

[q^0; 1] = T_1^0 [q^1; 1] = [1. 1. 0. 1.]

Columns of T_1^0:  x_1^0 = [0. 1. 0.]  y_1^0 = [-1.  0.  0.]  z_1^0 = [0. 0. 1.]  o_1^0 = [0. 1. 0.]

4.4.2 Composing HTMs

Chaining frames is matrix multiplication, exactly as in 2D:

TCA=TBA TCB\boxed{\mathbf{T}_C^A = \mathbf{T}_B^A\,\mathbf{T}_C^B}

TBA\mathbf{T}_B^A is the posture of frame {B}\{B\} relative to {A}\{A\}. The inner indices match and “cancel” (BB is once a subscript and once a superscript) — a quick way to check that you multiplied in the right order. For a robot arm: Tn0=T10 T21⋯Tnn−1\mathbf{T}_n^0 = \mathbf{T}_1^0\,\mathbf{T}_2^1\cdots\mathbf{T}_n^{n-1}.

Figure 4.16 — Composing HTMs: the inner indices cancel.

Example: frame {1}\{1\} is turned 45∘45^\circ about z0z_0 with origin (1,0,0)(1, 0, 0); frame {2}\{2\} is turned 30∘30^\circ about y1y_1 with origin (0.5,0,0.3)(0.5, 0, 0.3) in frame {1}\{1\}.

T_1^0 =
 [[ 0.7071 -0.7071  0.      1.    ]
 [ 0.7071  0.7071  0.      0.    ]
 [ 0.      0.      1.      0.    ]
 [ 0.      0.      0.      1.    ]]

T_2^1 =
 [[ 0.866  0.     0.5    0.5  ]
 [ 0.     1.     0.     0.   ]
 [-0.5    0.     0.866  0.3  ]
 [ 0.     0.     0.     1.   ]]

T_2^0 = T_1^0 T_2^1 =
 [[ 0.6124 -0.7071  0.3536  1.3536]
 [ 0.6124  0.7071  0.3536  0.3536]
 [-0.5     0.      0.866   0.3   ]
 [ 0.      0.      0.      1.    ]]
<Figure size 800x700 with 1 Axes>

4.4.3 The inverse of an HTM

T01=(T10)−1\mathbf{T}_0^1 = (\mathbf{T}_1^0)^{-1} describes frame {0}\{0\} seen from frame {1}\{1\}. You don’t need a general matrix inverse. Start from q0=R10q1+o10\mathbf{q}^0 = \mathbf{R}_1^0\mathbf{q}^1 + \mathbf{o}_1^0 and solve for q1\mathbf{q}^1: subtract o10\mathbf{o}_1^0, then multiply by (R10)⊤(\mathbf{R}_1^0)^\top (the inverse of a rotation):

q1=(R10)⊤q0−(R10)⊤o10⟹T01=(T10)−1=[(R10)⊤−(R10)⊤o100  0  01]\mathbf{q}^1 = (\mathbf{R}_1^0)^\top\mathbf{q}^0 - (\mathbf{R}_1^0)^\top\mathbf{o}_1^0 \quad\Longrightarrow\quad \boxed{\mathbf{T}_0^1 = \left(\mathbf{T}_1^0\right)^{-1} = \begin{bmatrix}(\mathbf{R}_1^0)^\top & -(\mathbf{R}_1^0)^\top\mathbf{o}_1^0\\ 0\ \ 0\ \ 0 & 1\end{bmatrix}}

Careful: the top-right block is not just −o10-\mathbf{o}_1^0. The offset has to be re-expressed in frame {1}\{1\}'s directions too.

T_0^1 = (T_1^0)^-1 =
 [[ 0.  1.  0. -1.]
 [-1.  0.  0. -0.]
 [ 0.  0.  1.  0.]
 [ 0.  0.  0.  1.]]

Same as np.linalg.inv? True
T_1^0 T_0^1 = I ?       True

[q^1; 1] = T_0^1 [q^0; 1] = [ 0. -1.  0.  1.]   (expected [0, -1, 0, 1])

4.4.4 Explore an HTM

Set the origin o10\mathbf{o}_1^0 and the orientation (as ZYX yaw–pitch–roll) of frame {1}\{1\}. The black point is fixed in frame {1}\{1\} at q1=(0.5, 0, 0)\mathbf{q}^1 = (0.5,\ 0,\ 0) — half a unit along x1x_1 — so it rides along with the frame. Its frame-{0}\{0\} coordinates are computed with q~0=T10q~1\tilde{\mathbf{q}}^0 = \mathbf{T}_1^0\tilde{\mathbf{q}}^1.

Loading...

Key Takeaways — Section 4.4

  • T10=[R10o100⊤1]\mathbf{T}_1^0 = \begin{bmatrix}\mathbf{R}_1^0 & \mathbf{o}_1^0\\ \mathbf{0}^\top & 1\end{bmatrix} (4×44\times4): its columns are x10,y10,z10,o10\mathbf{x}_1^0, \mathbf{y}_1^0, \mathbf{z}_1^0, \mathbf{o}_1^0.

  • Transform points with a single multiply: q~0=T10 q~1\tilde{\mathbf{q}}^0 = \mathbf{T}_1^0\,\tilde{\mathbf{q}}^1.

  • Chain frames: TCA=TBA TCB\mathbf{T}_C^A = \mathbf{T}_B^A\,\mathbf{T}_C^B (inner indices cancel).

  • Invert without a general inverse: T01=[(R10)⊤−(R10)⊤o100⊤1]\mathbf{T}_0^1 = \begin{bmatrix}(\mathbf{R}_1^0)^\top & -(\mathbf{R}_1^0)^\top\mathbf{o}_1^0\\ \mathbf{0}^\top & 1\end{bmatrix}.

4.5 Chapter Summary and Practice

Cheat sheet

What2D3D
Rotation matrixR10=[x10  y10]\mathbf{R}_1^0 = [\mathbf{x}_1^0\ \ \mathbf{y}_1^0], 2×22\times2R10=[x10  y10  z10]\mathbf{R}_1^0 = [\mathbf{x}_1^0\ \ \mathbf{y}_1^0\ \ \mathbf{z}_1^0], 3×33\times3
Convert a point (same origin)q0=R10q1\mathbf{q}^0 = \mathbf{R}_1^0\mathbf{q}^1same
Go backR01=(R10)⊤\mathbf{R}_0^1 = (\mathbf{R}_1^0)^\topsame
Moved + turned frameq0=R10q1+o10\mathbf{q}^0 = \mathbf{R}_1^0\mathbf{q}^1 + \mathbf{o}_1^0same
HTMT10=[R10o100⊤1]\mathbf{T}_1^0 = \begin{bmatrix}\mathbf{R}_1^0 & \mathbf{o}_1^0\\ \mathbf{0}^\top & 1\end{bmatrix}, 3×33\times3same blocks, 4×44\times4
Transform a pointq~0=T10q~1\tilde{\mathbf{q}}^0 = \mathbf{T}_1^0\tilde{\mathbf{q}}^1same
Chain framesTCA=TBATCB\mathbf{T}_C^A = \mathbf{T}_B^A\mathbf{T}_C^Bsame
Combine rotationsorder doesn’t matterorder matters; right = current axes

Looking ahead. Chapter 5 adds two things: six basic HTMs (Transx,a\mathrm{Trans}_{x,a}, Transy,b\mathrm{Trans}_{y,b}, Transz,c\mathrm{Trans}_{z,c}, Rotx,α\mathrm{Rot}_{x,\alpha}, Roty,β\mathrm{Rot}_{y,\beta}, Rotz,γ\mathrm{Rot}_{z,\gamma} — the pure translations and the HTM versions of Rx\mathbf{R}_x, Ry\mathbf{R}_y, Rz\mathbf{R}_z), and a three-step recipe that uses them to write Te0\mathbf{T}_e^0 for any robot arm.

Practice problems

Try each one on paper first, then expand the answer or check it with the code cell below.

P1 (2D HTM). Frame {1}\{1\} has origin o10=(2,0)\mathbf{o}_1^0 = (2, 0) and is turned by 90∘90^\circ. A point has q1=(1,0)\mathbf{q}^1 = (1, 0). Find q0\mathbf{q}^0.

Answer

\mathbf{R}_1^0 = \begin{bmatrix}0&-1\\1&0\end{bmatrix}, so \mathbf{q}^0 = \mathbf{R}_1^0\mathbf{q}^1 + \mathbf{o}_1^0 = (0, 1) + (2, 0) = (2, 1).

P2 (3D rotation matrix). x1x_1 points along −y0-y_0, y1y_1 points along x0x_0. Where does z1z_1 point? Write R10\mathbf{R}_1^0 and R01\mathbf{R}_0^1.

Answer

\mathbf{z}_1^0 = \mathbf{x}_1^0\times\mathbf{y}_1^0 = (0,-1,0)\times(1,0,0) = (0,0,1), so z_1 points along z_0. \mathbf{R}_1^0 = \begin{bmatrix}0&1&0\\-1&0&0\\0&0&1\end{bmatrix} = \mathbf{R}_z(-90^\circ), \ \mathbf{R}_0^1 = (\mathbf{R}_1^0)^\top = \begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix}.

P3 (order of rotations). A frame is first turned 90∘90^\circ about z0z_0, then 90∘90^\circ about its own new xx axis. Write the total rotation as a product of basic rotation matrices. Then do the same if the second turn is about the fixed x0x_0 axis.

Answer

Current (own) axis → multiply on the right: \mathbf{R} = \mathbf{R}_z(90^\circ)\,\mathbf{R}_x(90^\circ). Fixed axis → multiply on the left: \mathbf{R} = \mathbf{R}_x(90^\circ)\,\mathbf{R}_z(90^\circ). They are different matrices.

P4 (chaining HTMs). T10\mathbf{T}_1^0 has R10=Rz(90∘)\mathbf{R}_1^0 = \mathbf{R}_z(90^\circ), o10=(1,0,0)\mathbf{o}_1^0 = (1, 0, 0). T21\mathbf{T}_2^1 has R21=I\mathbf{R}_2^1 = \mathbf{I}, o21=(2,0,0)\mathbf{o}_2^1 = (2, 0, 0). Where is the origin of frame {2}\{2\} in frame {0}\{0\}?

Answer

\mathbf{o}_2^0 = \mathbf{R}_1^0\,\mathbf{o}_2^1 + \mathbf{o}_1^0 = (0, 2, 0) + (1, 0, 0) = (1, 2, 0) — the last column of \mathbf{T}_2^0 = \mathbf{T}_1^0\mathbf{T}_2^1.

P1: q^0 = [2. 1.]

P2: R_1^0 =
 [[ 0  1  0]
 [-1  0  0]
 [ 0  0  1]] 
    equals R_z(-90°)? True

P3: current axis R_z R_x =
 [[ 0. -0.  1.]
 [ 1.  0. -0.]
 [ 0.  1.  0.]]
    fixed axis   R_x R_z =
 [[ 0. -1.  0.]
 [ 0.  0. -1.]
 [ 1.  0.  0.]]

P4: o_2^0 = [1. 2. 0.]