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 5 — Forward Position Kinematics

Chapter 4 gave us the homogeneous transformation matrix (HTM): one 4×44\times4 matrix that stores both the position and the orientation of a frame, and that chains frames together by plain matrix multiplication. This chapter turns that tool into a recipe that works for any serial robot arm:

Forward position kinematics: given the joint variables θ1,…,θn\theta_1,\dots,\theta_n, find the pose (position and orientation) of the gripper, Te0\mathbf{T}_e^0.

The recipe has three steps — set up frames, write one HTM per link, multiply — and we apply it to five robots of increasing complexity, ending with a real industrial arm (the UR5e) whose geometry comes straight from a MuJoCo model file.

SectionTopic
5.1The general HTM and the six basic HTMs
5.2Composition of HTMs, and how a joint moves a frame
5.3The three-step forward kinematics procedure
5.4Example 1 — planar 2R arm
5.5Example 2 — 3R spherical wrist
5.6Example 3 — RPP cylindrical robot
5.7Example 4 — SCARA robot
5.8Example 5 — UR5e from a MuJoCo model
5.9The FK Frame Animator and chapter summary

Notation used in this chapter. TCA\mathbf{T}_C^A is the pose of frame {C}\{C\} relative to frame {A}\{A\}: the subscript names the frame being described, the superscript names the reference frame. Joint variables are θi\theta_i; for a prismatic joint we usually write did_i instead, to remind us it is a length.

5.1 The General HTM and the Basic HTMs

5.1.1 Reading an HTM column by column

Every HTM has the same block structure. For frame {1}\{1\} described in frame {0}\{0\}:

T10=[nxsxaxdxnysyaydynzszazdz0001]=[nsad0001]4×4\mathbf{T}_1^0 = \begin{bmatrix} n_x & s_x & a_x & d_x\\ n_y & s_y & a_y & d_y\\ n_z & s_z & a_z & d_z\\ 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} \mathbf{n} & \mathbf{s} & \mathbf{a} & \mathbf{d}\\ 0 & 0 & 0 & 1 \end{bmatrix}_{4\times4}
ColumnMeaning
n\mathbf{n}unit vector along x1x_1, expressed in frame {0}\{0\}
s\mathbf{s}unit vector along y1y_1, expressed in frame {0}\{0\}
a\mathbf{a}unit vector along z1z_1, expressed in frame {0}\{0\}
d\mathbf{d}coordinates of the origin o1o_1, expressed in frame {0}\{0\}

Figure 5.1 — The columns of T10 are the axes x1, y1, z1 (n, s, a) and the origin o1 (d), all written in frame {0}.

The first three columns form the rotation matrix R10\mathbf{R}_1^0 (orientation); the last column is the position. So you can read a frame’s pose directly off its HTM: column 3 tells you where z1z_1 points, column 4 tells you where o1o_1 is.

5.1.2 The six basic HTMs

Almost every HTM we write in this chapter is a product of six basic HTMs: a pure translation along one axis, or a pure rotation about one axis.

Translation along xx, yy, zz by aa, bb, cc:

Transx,a=[100a010000100001],Transy,b=[1000010b00100001],Transz,c=[10000100001c0001]\mathrm{Trans}_{x,a}=\begin{bmatrix}1&0&0&a\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix},\quad \mathrm{Trans}_{y,b}=\begin{bmatrix}1&0&0&0\\0&1&0&b\\0&0&1&0\\0&0&0&1\end{bmatrix},\quad \mathrm{Trans}_{z,c}=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&1&c\\0&0&0&1\end{bmatrix}

Rotation about xx, yy, zz by α\alpha, β\beta, γ\gamma (with cα=cos⁡αc_\alpha=\cos\alpha, sα=sin⁡αs_\alpha=\sin\alpha, etc.):

Rotx,α=[10000cα−sα00sαcα00001],Roty,β=[cβ0sβ00100−sβ0cβ00001],Rotz,γ=[cγ−sγ00sγcγ0000100001]\mathrm{Rot}_{x,\alpha}=\begin{bmatrix}1&0&0&0\\0&c_\alpha&-s_\alpha&0\\0&s_\alpha&c_\alpha&0\\0&0&0&1\end{bmatrix},\quad \mathrm{Rot}_{y,\beta}=\begin{bmatrix}c_\beta&0&s_\beta&0\\0&1&0&0\\-s_\beta&0&c_\beta&0\\0&0&0&1\end{bmatrix},\quad \mathrm{Rot}_{z,\gamma}=\begin{bmatrix}c_\gamma&-s_\gamma&0&0\\s_\gamma&c_\gamma&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}

Figure 5.2 — The six basic HTMs: a pure translation along, or a pure rotation about, one axis.

A translation HTM has I3×3\mathbf{I}_{3\times3} in the rotation block; a rotation HTM has the basic rotation matrix from Chapter 4 (Rx\mathbf{R}_x, Ry\mathbf{R}_y, Rz\mathbf{R}_z) in the rotation block and zero translation.

Trans_y,0.5 =
[[1.  0.  0.  0. ]
 [0.  1.  0.  0.5]
 [0.  0.  1.  0. ]
 [0.  0.  0.  1. ]]

Rot_x,90° =
[[ 1.  0.  0.  0.]
 [ 0.  0. -1.  0.]
 [ 0.  1.  0.  0.]
 [ 0.  0.  0.  1.]]
<Figure size 1300x800 with 6 Axes>

Key Takeaways — Section 5.1

  • The columns of T10=[n s a d]\mathbf{T}_1^0 = [\mathbf{n}\ \mathbf{s}\ \mathbf{a}\ \mathbf{d}] are the x1x_1, y1y_1, z1z_1 unit vectors and the origin o1o_1, all expressed in frame {0}\{0\}.

  • Six basic HTMs — Transx\mathrm{Trans}_{x}, Transy\mathrm{Trans}_{y}, Transz\mathrm{Trans}_{z}, Rotx\mathrm{Rot}_{x}, Roty\mathrm{Rot}_{y}, Rotz\mathrm{Rot}_{z} — are the building blocks for every HTM in this chapter.

5.2 Composition of HTMs, and How a Joint Moves a Frame

5.2.1 Composition

If we know frame {B}\{B\} relative to {A}\{A\} and frame {C}\{C\} relative to {B}\{B\}, then

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

The inner indices match and “cancel” (BB appears once as a subscript and once as a superscript), which is an easy way to check that a product is written in the right order.

5.2.2 Reading a product of basic HTMs

A product of basic HTMs can be read left to right as a sequence of moves of a frame, where every move is made along or about the frame’s current axes (this is the post-multiplication / body-axis rule from Chapter 4, §4.3.2). For example

T21=Transy,1  Rotz,90∘  Roty,90∘\mathbf{T}_2^1 = \mathrm{Trans}_{y,1}\;\mathrm{Rot}_{z,90^\circ}\;\mathrm{Rot}_{y,90^\circ}

means: start at frame {1}\{1\}, slide 1 along yy, then rotate 90∘90^\circ about the new zz, then rotate 90∘90^\circ about the newest yy. Where the frame ends up is frame {2}\{2\}.

Figure 5.3 — Reading T21 = Transy,1 Rotz,90° Roty,90° left to right: each move uses the frame’s current axes.

5.2.3 Worked example (Lecture 8)

Frame {1}\{1\} sits 1 unit above the ground frame {0}\{0\} with the same orientation, and frame {2}\{2\} is obtained from frame {1}\{1\} by the three moves above:

T10=Transz,1,T21=Transy,1 Rotz,90∘ Roty,90∘=[0−1000011−10000001].\mathbf{T}_1^0 = \mathrm{Trans}_{z,1}, \qquad \mathbf{T}_2^1 = \mathrm{Trans}_{y,1}\,\mathrm{Rot}_{z,90^\circ}\,\mathrm{Rot}_{y,90^\circ} = \begin{bmatrix}0&-1&0&0\\0&0&1&1\\-1&0&0&0\\0&0&0&1\end{bmatrix}.

Figure 5.4 — Lecture 8 example: frame {1} is 1 above {0}; frame {2} is reached by the moves of Figure 5.3; q is 1 along z2.

A point qq sits 1 unit along z2z_2: q2=(0,0,1)q^2 = (0,0,1). Where is it in frame {0}\{0\}? Augment with a 1 and multiply:

[q01]=T20[q21],T20=T10 T21.\begin{bmatrix}q^0\\1\end{bmatrix} = \mathbf{T}_2^0\begin{bmatrix}q^2\\1\end{bmatrix}, \qquad \mathbf{T}_2^0 = \mathbf{T}_1^0\,\mathbf{T}_2^1 .
T_2^1 =
[[ 0. -1.  0.  0.]
 [ 0.  0.  1.  1.]
 [-1.  0.  0.  0.]
 [ 0.  0.  0.  1.]]

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

q^2 = [0 0 1]   q^1 = [0. 2. 0.]   q^0 = [0. 2. 1.]
Matches the lecture result q^0 = (0, 2, 1)? True

Below, the slider applies the factors of T20=Transz,1⋅Transy,1 Rotz,90∘ Roty,90∘\mathbf{T}_2^0 = \mathrm{Trans}_{z,1}\cdot \mathrm{Trans}_{y,1}\,\mathrm{Rot}_{z,90^\circ}\,\mathrm{Rot}_{y,90^\circ} one at a time. The bold frame with a black dot is the moving frame after the factors applied so far; faded frames are earlier intermediate stops, and a labeled frame appears each time a Tii−1\mathbf{T}_i^{i-1} is complete. Watch how each rotation acts about the moving frame’s current axis.

The two helper functions in the next cell are reused for every robot in this chapter. A product is stored as a list of groups — one group per Tii−1\mathbf{T}_i^{i-1} — and each group is a list of basic-HTM factors.

Loading...

5.2.4 How a joint moves a frame (Lecture 8)

Now attach frame {1}\{1\} to a link that turns on a revolute joint whose axis is z0z_0. At the home configuration (θ=0\theta = 0) the frame is T10\mathbf{T}_1^0. When the joint turns by θ\theta, the link — and the frame glued to it — rotates about the joint axis to a new frame {1′}\{1'\}:

T1′0=T10 T1′1=T10 Rotz,θ.\mathbf{T}_{1'}^0 = \mathbf{T}_1^0\,\mathbf{T}_{1'}^1 = \mathbf{T}_1^0\,\mathrm{Rot}_{z,\theta}.

Figure 5.5 — A revolute joint about z1 turns frame {1} into {1′}: post-multiply by Rotz,θ.

This only works as a single post-multiplied Rotz,θ\mathrm{Rot}_{z,\theta} because we chose z1z_1 to lie along the joint axis: rotating about the joint axis is then the same as rotating about the frame’s own current zz-axis. That choice is the key idea of the whole procedure in Section 5.3.

Key Takeaways — Section 5.2

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

  • Read a product of basic HTMs left to right: each factor moves the frame along or about its current axes.

  • Augment a point with a 1 to transform it: [q0;1]=T20 [q2;1][q^0;1] = \mathbf{T}_2^0\,[q^2;1].

  • If ziz_i lies along joint ii’s axis, turning the joint by θ\theta just post-multiplies the frame by Rotz,θ\mathrm{Rot}_{z,\theta}.

5.3 The Forward Kinematics Procedure

For an nn-joint robotic arm:

Step 1 — Set up a frame for each link.

  • Frame {0}\{0\}: fixed to the ground (the inertial frame).

  • Frames {1},…,{n}\{1\},\dots,\{n\}: frame {i}\{i\} is fixed to link ii and moves with joint ii. Put ziz_i along the axis of joint ii (the rotation axis for a revolute joint, the sliding direction for a prismatic joint).

  • Frame {e}\{e\}: at the gripper (end-effector) center. Because there is no joint between frame {n}\{n\} and frame {e}\{e\}, Ten\mathbf{T}_e^n is constant.

Figure 5.6 — Step 1: frame {0} on the ground, frame {i} on link i with zi along joint i, frame {e} at the gripper.

Step 2 — Obtain Tii−1(θi)\mathbf{T}_i^{i-1}(\theta_i) for i=1,…,ni = 1,\dots,n.

  • Home configuration (θi=0\theta_i = 0 for all ii): write Tii−1(0)\mathbf{T}_i^{i-1}(0) as a product of basic HTMs that moves frame {i−1}\{i-1\} onto frame {i}\{i\}.

  • Joint motion (θi≠0\theta_i \neq 0): post-multiply by one basic HTM along/about ziz_i:

revolute joint:Tii−1(θi)=Tii−1(0) Rotz,θiprismatic joint:Tii−1(θi)=Tii−1(0) Transz,θi\boxed{ \begin{aligned} \text{revolute joint:}\quad & \mathbf{T}_i^{i-1}(\theta_i) = \mathbf{T}_i^{i-1}(0)\,\mathrm{Rot}_{z,\theta_i}\\ \text{prismatic joint:}\quad & \mathbf{T}_i^{i-1}(\theta_i) = \mathbf{T}_i^{i-1}(0)\,\mathrm{Trans}_{z,\theta_i} \end{aligned}}

Step 3 — Multiply.

Te0=T10(θ1) T21(θ2)⋯Tnn−1(θn) Ten\boxed{\mathbf{T}_e^0 = \mathbf{T}_1^0(\theta_1)\,\mathbf{T}_2^1(\theta_2)\cdots\mathbf{T}_n^{n-1}(\theta_n)\,\mathbf{T}_e^n}

The position of the gripper is the last column of Te0\mathbf{T}_e^0; its orientation is the upper-left 3×33\times3 block.

Note for readers of other textbooks. Many books use the Denavit–Hartenberg (DH) convention, which restricts how frames may be placed so that each link needs exactly four parameters. Our procedure lets you place frames freely (only ziz_i along joint ii is required) and writes Tii−1(0)\mathbf{T}_i^{i-1}(0) with any basic HTMs you like. Both give the same Te0\mathbf{T}_e^0; ours is easier to set up from a drawing or a CAD/MJCF model.

The class below is a direct translation of Steps 2 and 3 into code. Each example in this chapter only has to supply the home factors of every Tii−1(0)\mathbf{T}_i^{i-1}(0), the joint types, and the constant tool factors of Ten\mathbf{T}_e^n.

Key Takeaways — Section 5.3

  • Step 1: frame {0}\{0\} on the ground, frame {i}\{i\} on link ii with ziz_i along joint ii, frame {e}\{e\} at the gripper.

  • Step 2: write the home transform Tii−1(0)\mathbf{T}_i^{i-1}(0) from basic HTMs, then post-multiply by Rotz,θi\mathrm{Rot}_{z,\theta_i} (revolute) or Transz,θi\mathrm{Trans}_{z,\theta_i} (prismatic).

  • Step 3: Te0=T10T21⋯Tnn−1Ten\mathbf{T}_e^0 = \mathbf{T}_1^0\mathbf{T}_2^1\cdots\mathbf{T}_n^{n-1}\mathbf{T}_e^n.

  • The same three steps work for any serial robot, from a 2R arm to a 6-DOF industrial arm.

5.4 Example 1 — Planar 2R Arm

Two revolute joints with parallel axes (out of the page), link lengths L1L_1 and L2L_2. This is the Chapter 3 robot, so we already know the answer — a good first test of the procedure.

Step 1 — Frames. All zz-axes point out of the page (along the joint axes). At home the arm is stretched along x0x_0: frame {1}\{1\} coincides with {0}\{0\}, frame {2}\{2\} is at the elbow, frame {e}\{e\} at the tip. All frames stay parallel at home.

Figure 5.7 — Example 1 (Lecture 9): frames of the planar 2R arm at home (a) and after the joints move (b).

Step 2 — Home configuration and joint motion.

T10(0)=I4×4,T21(0)=Transx,L1,Te2=Transx,L2\mathbf{T}_1^0(0) = \mathbf{I}_{4\times4},\qquad \mathbf{T}_2^1(0) = \mathrm{Trans}_{x,L_1},\qquad \mathbf{T}_e^2 = \mathrm{Trans}_{x,L_2}
T10(θ1)=T10(0) Rotz,θ1=Rotz,θ1,T21(θ2)=T21(0) Rotz,θ2=Transx,L1Rotz,θ2\mathbf{T}_1^0(\theta_1) = \mathbf{T}_1^0(0)\,\mathrm{Rot}_{z,\theta_1} = \mathrm{Rot}_{z,\theta_1},\qquad \mathbf{T}_2^1(\theta_2) = \mathbf{T}_2^1(0)\,\mathrm{Rot}_{z,\theta_2} = \mathrm{Trans}_{x,L_1}\mathrm{Rot}_{z,\theta_2}

Step 3 — Multiply.

Te0=T10(θ1) T21(θ2) Te2=Rotz,θ1 Transx,L1 Rotz,θ2 Transx,L2\mathbf{T}_e^0 = \mathbf{T}_1^0(\theta_1)\,\mathbf{T}_2^1(\theta_2)\,\mathbf{T}_e^2 = \mathrm{Rot}_{z,\theta_1}\,\mathrm{Trans}_{x,L_1}\,\mathrm{Rot}_{z,\theta_2}\,\mathrm{Trans}_{x,L_2}

Read it left to right: turn by θ1\theta_1, walk L1L_1, turn by θ2\theta_2, walk L2L_2.

T_e^0 at (θ1, θ2) = (90°, -90°) =
[[1.  0.  0.  0.7]
 [0.  1.  0.  1. ]
 [0.  0.  1.  0. ]
 [0.  0.  0.  1. ]]

Chapter 3 formula: (x, y) = (0.7000, 1.0000)   matches column 4? True
Loading...

with c1=cos⁡θ1c_1 = \cos\theta_1, s1=sin⁡θ1s_1 = \sin\theta_1, c12=cos⁡(θ1+θ2)c_{12} = \cos(\theta_1+\theta_2), s12=sin⁡(θ1+θ2)s_{12} = \sin(\theta_1+\theta_2). The last column is exactly the Chapter 3 result x=L1c1+L2c12x = L_1c_1 + L_2c_{12}, y=L1s1+L2s12y = L_1s_1 + L_2s_{12} — and we get the orientation (a rotation of θ1+θ2\theta_1+\theta_2 about zz) for free.

Robot.fk matches fk_2R: True
<Figure size 1300x550 with 2 Axes>
Loading...

Key Takeaways — Section 5.4

  • Te0=Rotz,θ1Transx,L1Rotz,θ2Transx,L2\mathbf{T}_e^0 = \mathrm{Rot}_{z,\theta_1}\mathrm{Trans}_{x,L_1}\mathrm{Rot}_{z,\theta_2}\mathrm{Trans}_{x,L_2}: “turn, walk, turn, walk”.

  • The position column reproduces the Chapter 3 formula; the rotation block adds the orientation θ1+θ2\theta_1+\theta_2.

5.5 Example 2 — 3R Spherical Wrist

Three revolute joints whose axes all meet at one point (the wrist center): roll, pitch, roll. A gripper sits a distance LL beyond the wrist center along the last axis.

Step 1 — Frames. Put every ziz_i along joint ii. Because the axes intersect, all three origins are at the wrist center: o0=o1=o2=o3o_0 = o_1 = o_2 = o_3. At home, z1z_1 and z3z_3 point up (along z0z_0) and z2z_2 points along y0y_0, the pitch axis.

Figure 5.8 — Example 2 (Lecture 9): the spherical wrist. All joint axes meet at the wrist center.

Step 2 — Home configuration and joint motion.

T10(0)=I4×4,T21(0)=Rotx,−90∘,T32(0)=Rotx,90∘,Te3=Transz,L\mathbf{T}_1^0(0) = \mathbf{I}_{4\times4},\qquad \mathbf{T}_2^1(0) = \mathrm{Rot}_{x,-90^\circ},\qquad \mathbf{T}_3^2(0) = \mathrm{Rot}_{x,90^\circ},\qquad \mathbf{T}_e^3 = \mathrm{Trans}_{z,L}

Rotx,−90∘\mathrm{Rot}_{x,-90^\circ} tips zz over so that z2z_2 lies along the pitch axis, and Rotx,90∘\mathrm{Rot}_{x,90^\circ} tips it back up for the third (roll) joint. Then

T10(θ1)=Rotz,θ1,T21(θ2)=Rotx,−90∘Rotz,θ2,T32(θ3)=Rotx,90∘Rotz,θ3.\mathbf{T}_1^0(\theta_1) = \mathrm{Rot}_{z,\theta_1},\quad \mathbf{T}_2^1(\theta_2) = \mathrm{Rot}_{x,-90^\circ}\mathrm{Rot}_{z,\theta_2},\quad \mathbf{T}_3^2(\theta_3) = \mathrm{Rot}_{x,90^\circ}\mathrm{Rot}_{z,\theta_3}.

Step 3 — Multiply.

Te0=Rotz,θ1  Rotx,−90∘Rotz,θ2  Rotx,90∘Rotz,θ3  Transz,L\mathbf{T}_e^0 = \mathrm{Rot}_{z,\theta_1}\;\mathrm{Rot}_{x,-90^\circ}\mathrm{Rot}_{z,\theta_2}\;\mathrm{Rot}_{x,90^\circ}\mathrm{Rot}_{z,\theta_3}\;\mathrm{Trans}_{z,L}
T_e^0 at (θ1, θ2, θ3) = (0, 90°, 0) =
[[ 0.   0.   1.   0.6]
 [ 0.   1.   0.   0. ]
 [-1.   0.   0.   0. ]
 [ 0.   0.   0.   1. ]]
Loading...
Rot_x,-90 · Rot_z,θ2 · Rot_x,90  ==  Rot_y,θ2 ? True

Two things to notice:

  • The gripper position is oe=L (c1s2, s1s2, c2)o_e = L\,(c_1s_2,\ s_1s_2,\ c_2) — a point on a sphere of radius LL, which is why this is called a spherical wrist. θ3\theta_3 does not move the gripper’s position at all; it only spins the gripper about its own axis.

  • Rotx,−90∘Rotz,θ2Rotx,90∘=Roty,θ2\mathrm{Rot}_{x,-90^\circ}\mathrm{Rot}_{z,\theta_2}\mathrm{Rot}_{x,90^\circ} = \mathrm{Rot}_{y,\theta_2}, so the orientation is Re0=Rz(θ1)Ry(θ2)Rz(θ3)\mathbf{R}_e^0 = \mathbf{R}_z(\theta_1)\mathbf{R}_y(\theta_2)\mathbf{R}_z(\theta_3) — exactly the ZYZ Euler angles. A spherical wrist can reach any orientation, which is why most 6-DOF arms end with one.

Robot.fk matches fk_wrist: True
<Figure size 1300x550 with 2 Axes>
Loading...

Key Takeaways — Section 5.5

  • When joint axes intersect, several frames share one origin; the home transforms are then pure rotations.

  • Fixed rotations in Tii−1(0)\mathbf{T}_i^{i-1}(0) (here Rotx,∓90∘\mathrm{Rot}_{x,\mp90^\circ}) exist only to point ziz_i along joint ii.

  • Spherical wrist: position L(c1s2,s1s2,c2)L(c_1s_2, s_1s_2, c_2), orientation RzRyRz\mathbf{R}_z\mathbf{R}_y\mathbf{R}_z (ZYZ Euler angles).

5.6 Example 3 — RPP Cylindrical Robot

A revolute base θ1\theta_1, a prismatic vertical lift d2d_2, and a prismatic horizontal reach d3d_3. The shoulder is at height hh and the reach slide starts a distance aa from the column.

Step 1 — Frames. z1z_1 along the base axis (up). z2z_2 along the lift direction (up), at height hh. z3z_3 along the reach direction, which at home is x0x_0; the gripper is at the end of the reach slide, so frame {e}\{e\} coincides with frame {3}\{3\}.

Figure 5.9 — Example 3: the RPP cylindrical robot, with base height h, lift d2, offset a and reach d3.

Step 2 — Home configuration and joint motion.

T10(0)=I4×4,T21(0)=Transz,h,T32(0)=Transx,a Roty,90∘,Te3=I4×4\mathbf{T}_1^0(0) = \mathbf{I}_{4\times4},\qquad \mathbf{T}_2^1(0) = \mathrm{Trans}_{z,h},\qquad \mathbf{T}_3^2(0) = \mathrm{Trans}_{x,a}\,\mathrm{Rot}_{y,90^\circ},\qquad \mathbf{T}_e^3 = \mathbf{I}_{4\times4}

Roty,90∘\mathrm{Rot}_{y,90^\circ} turns zz to point along the old xx so that z3z_3 lies along the reach slide. Joints 2 and 3 are prismatic, so they post-multiply by Transz\mathrm{Trans}_z:

T10(θ1)=Rotz,θ1,T21(d2)=Transz,h Transz,d2,T32(d3)=Transx,a Roty,90∘ Transz,d3.\mathbf{T}_1^0(\theta_1) = \mathrm{Rot}_{z,\theta_1},\quad \mathbf{T}_2^1(d_2) = \mathrm{Trans}_{z,h}\,\mathrm{Trans}_{z,d_2},\quad \mathbf{T}_3^2(d_3) = \mathrm{Trans}_{x,a}\,\mathrm{Rot}_{y,90^\circ}\,\mathrm{Trans}_{z,d_3}.

Step 3 — Multiply.

Te0=Rotz,θ1  Transz,hTransz,d2  Transx,aRoty,90∘Transz,d3\mathbf{T}_e^0 = \mathrm{Rot}_{z,\theta_1}\;\mathrm{Trans}_{z,h}\mathrm{Trans}_{z,d_2}\;\mathrm{Trans}_{x,a}\mathrm{Rot}_{y,90^\circ}\mathrm{Trans}_{z,d_3}
T_e^0 at (θ1, d2, d3) = (90°, 0, 0) =
[[ 0.  -1.   0.   0. ]
 [ 0.   0.   1.   0.7]
 [-1.   0.   0.   0.8]
 [ 0.   0.   0.   1. ]]
Loading...

The gripper position oe=((a+d3)c1, (a+d3)s1, h+d2)o_e = \big((a+d_3)c_1,\ (a+d_3)s_1,\ h+d_2\big) is exactly cylindrical coordinates: radius a+d3a+d_3, angle θ1\theta_1, height h+d2h+d_2 — hence the name. Notice the prismatic variables enter the position linearly, while the revolute variable enters through cos⁡\cos and sin⁡\sin.

Robot.fk matches fk_rpp: True
<Figure size 1300x550 with 2 Axes>
Loading...

Key Takeaways — Section 5.6

  • A prismatic joint post-multiplies by Transz,di\mathrm{Trans}_{z,d_i} instead of Rotz,θi\mathrm{Rot}_{z,\theta_i} — that is the only change.

  • If no tool offset is needed, frame {e}\{e\} can coincide with frame {n}\{n\}: Ten=I\mathbf{T}_e^n = \mathbf{I}.

  • RPP robot: oe=((a+d3)c1,(a+d3)s1,h+d2)o_e = ((a+d_3)c_1, (a+d_3)s_1, h+d_2) — cylindrical coordinates.

5.7 Example 4 — SCARA Robot (RRPR)

The SCARA (Selective Compliance Assembly Robot Arm) is the standard pick-and-place robot: two vertical revolute joints (θ1\theta_1, θ2\theta_2) move the arm in a horizontal plane, a prismatic quill (d3d_3) moves the tool down, and a final revolute joint (θ4\theta_4) rolls the tool. Parameters: shoulder height hh, link lengths L1L_1, L2L_2, and tool length ded_e.

Step 1 — Frames. z1z_1 and z2z_2 point up along the two vertical revolute axes. The quill moves downward, so we flip the frame with Rotx,180∘\mathrm{Rot}_{x,180^\circ}: z3z_3 and z4z_4 point down. Frame {e}\{e\} is at the tool tip, ded_e further along z4z_4.

Figure 5.10 — Example 4 (Lecture 10): SCARA frames at home. Frames {3}, {4}, {e} point z down.

Step 2 — Home configuration and joint motion.

T10(0)=Transz,h,T21(0)=Transx,L1,T32(0)=Transx,L2 Rotx,180∘,T43(0)=I4×4,Te4=Transz,de\mathbf{T}_1^0(0) = \mathrm{Trans}_{z,h},\quad \mathbf{T}_2^1(0) = \mathrm{Trans}_{x,L_1},\quad \mathbf{T}_3^2(0) = \mathrm{Trans}_{x,L_2}\,\mathrm{Rot}_{x,180^\circ},\quad \mathbf{T}_4^3(0) = \mathbf{I}_{4\times4},\quad \mathbf{T}_e^4 = \mathrm{Trans}_{z,d_e}
T10(θ1)=Transz,h Rotz,θ1,T21(θ2)=Transx,L1 Rotz,θ2,T32(d3)=Transx,L2 Rotx,180∘ Transz,d3  (prismatic),T43(θ4)=Rotz,θ4.\begin{aligned} \mathbf{T}_1^0(\theta_1) &= \mathrm{Trans}_{z,h}\,\mathrm{Rot}_{z,\theta_1}, & \mathbf{T}_2^1(\theta_2) &= \mathrm{Trans}_{x,L_1}\,\mathrm{Rot}_{z,\theta_2},\\ \mathbf{T}_3^2(d_3) &= \mathrm{Trans}_{x,L_2}\,\mathrm{Rot}_{x,180^\circ}\,\mathrm{Trans}_{z,d_3}\ \ \text{(prismatic)}, & \mathbf{T}_4^3(\theta_4) &= \mathrm{Rot}_{z,\theta_4}. \end{aligned}

Step 3 — Multiply.

Te0=T10 T21 T32 T43 Te4\mathbf{T}_e^0 = \mathbf{T}_1^0\,\mathbf{T}_2^1\,\mathbf{T}_3^2\,\mathbf{T}_4^3\,\mathbf{T}_e^4
T_e^0 at (θ1, θ2, d3, θ4) = (90°, -90°, 0, 0) =
[[ 1.    0.    0.    0.6 ]
 [ 0.   -1.    0.    0.8 ]
 [ 0.    0.   -1.    0.65]
 [ 0.    0.    0.    1.  ]]
Loading...

Reading the result:

  • Position: oe=(L1c1+L2c12, L1s1+L2s12, h−d3−de)o_e = \big(L_1c_1 + L_2c_{12},\ L_1s_1 + L_2s_{12},\ h - d_3 - d_e\big). The x,yx,y part is the planar 2R arm from Example 1; the height only depends on the quill. Positive d3d_3 lowers the tool because z3z_3 points down.

  • Orientation: with φ=θ1+θ2−θ4\varphi = \theta_1 + \theta_2 - \theta_4, Re0=[cos⁡φsin⁡φ0sin⁡φ−cos⁡φ000−1]\mathbf{R}_e^0 = \begin{bmatrix}\cos\varphi & \sin\varphi & 0\\ \sin\varphi & -\cos\varphi & 0\\ 0&0&-1\end{bmatrix}: the tool always points straight down (ze=−z0z_e = -z_0) and can only spin about the vertical. θ4\theta_4 enters with a minus sign because z4z_4 points down, so a positive θ4\theta_4 turns the tool clockwise when seen from above.

Robot.fk matches fk_scara: True
<Figure size 1300x550 with 2 Axes>
Loading...

Key Takeaways — Section 5.7

  • A fixed Rotx,180∘\mathrm{Rot}_{x,180^\circ} in T32(0)\mathbf{T}_3^2(0) flips zz downward so the quill’s positive direction is “down”.

  • SCARA: oe=(L1c1+L2c12, L1s1+L2s12, h−d3−de)o_e = (L_1c_1 + L_2c_{12},\ L_1s_1 + L_2s_{12},\ h-d_3-d_e); the tool always points down with yaw θ1+θ2−θ4\theta_1+\theta_2-\theta_4.

5.8 Example 5 — UR5e from a MuJoCo Model

The Universal Robots UR5e is a 6-DOF industrial arm. Its geometry here comes from the MuJoCo Menagerie model universal_robots_ur5e/ur5e.xml, the same kind of MJCF file you met in Chapter 3. The MJCF uses its own body frames (four of its joint axes are along a local yy), so we re-assign the frames with our Step 1 recipe: ziz_i along joint ii, pointing the same way as the MJCF joint axis, so that θi\theta_i equals MuJoCo’s qpos[i]. Frame {0}\{0\} is the MuJoCo world frame; at home the arm reaches along −x0-x_0.

Parameters (m): d1=0.163d_1 = 0.163, a2=0.425a_2 = 0.425, a3=0.392a_3 = 0.392, d4=0.134d_4 = 0.134, d5=0.100d_5 = 0.100, d6=0.100d_6 = 0.100.

Step 1 — Frames at home (θ=0\theta = 0).

FrameMJCF joint / siteoio_i in frame {0}\{0\}ziz_i
1shoulder_pan_joint(0, 0, 0.163)(0,\ 0,\ 0.163)+z0+z_0
2shoulder_lift_joint(0, 0, 0.163)(0,\ 0,\ 0.163)−y0-y_0
3elbow_joint(−0.425, 0, 0.163)(-0.425,\ 0,\ 0.163)−y0-y_0
4wrist_1_joint(−0.817, 0, 0.163)(-0.817,\ 0,\ 0.163)−y0-y_0
5wrist_2_joint(−0.817, −0.134, 0.163)(-0.817,\ -0.134,\ 0.163)−z0-z_0
6wrist_3_joint(−0.817, −0.134, 0.063)(-0.817,\ -0.134,\ 0.063)−y0-y_0
eattachment_site(−0.817, −0.234, 0.063)(-0.817,\ -0.234,\ 0.063)−y0-y_0

Axes 2, 3 and 4 are parallel, and axis 2 meets axis 1, so we may slide o2o_2 along its axis until o2=o1o_2 = o_1. The small sideways offsets in the MJCF (0.138, -0.131, 0.127 m) then collapse into the single offset d4=0.134d_4 = 0.134 m.

Figure 5.11 — Example 5: UR5e at home with frames re-assigned so zi lies along joint i.

Step 2 — Home configuration and joint motion (all joints revolute):

T10(0)=Transz,d1,T21(0)=Rotx,90∘,T32(0)=Transx,−a2,T43(0)=Transx,−a3,T54(0)=Transz,d4 Rotx,90∘,T65(0)=Transz,d5 Rotx,−90∘,Te6=Transz,d6,Tii−1(θi)=Tii−1(0) Rotz,θi.\begin{aligned} \mathbf{T}_1^0(0) &= \mathrm{Trans}_{z,d_1}, & \mathbf{T}_2^1(0) &= \mathrm{Rot}_{x,90^\circ}, & \mathbf{T}_3^2(0) &= \mathrm{Trans}_{x,-a_2},\\ \mathbf{T}_4^3(0) &= \mathrm{Trans}_{x,-a_3}, & \mathbf{T}_5^4(0) &= \mathrm{Trans}_{z,d_4}\,\mathrm{Rot}_{x,90^\circ}, & \mathbf{T}_6^5(0) &= \mathrm{Trans}_{z,d_5}\,\mathrm{Rot}_{x,-90^\circ},\\ \mathbf{T}_e^6 &= \mathrm{Trans}_{z,d_6}, & \mathbf{T}_i^{i-1}(\theta_i) &= \mathbf{T}_i^{i-1}(0)\,\mathrm{Rot}_{z,\theta_i}. \end{aligned}

Step 3 — Multiply. Te0=T10T21T32T43T54T65Te6\mathbf{T}_e^0 = \mathbf{T}_1^0\mathbf{T}_2^1\mathbf{T}_3^2\mathbf{T}_4^3\mathbf{T}_5^4\mathbf{T}_6^5\mathbf{T}_e^6.

This chain was checked against MuJoCo’s own forward kinematics (data.site_xpos, data.site_xmat of attachment_site) at 500 random joint vectors: position and orientation agree to about 10-15.

T_e^0 at home (q = 0) =
[[ 1.     0.     0.    -0.817]
 [ 0.     0.    -1.    -0.234]
 [ 0.     1.     0.     0.063]
 [ 0.     0.     0.     1.   ]]

T_e^0 at the 'upright' pose (0, -90°, 0, -90°, 0, 0) =
[[-1.     0.     0.     0.   ]
 [ 0.     0.    -1.    -0.234]
 [ 0.    -1.     0.     1.08 ]
 [ 0.     0.     0.     1.   ]]
Loading...

The closed form is long, but notice the structure: θ2\theta_2, θ3\theta_3, θ4\theta_4 only appear through the sums θ23=θ2+θ3\theta_{23} = \theta_2+\theta_3 and θ234=θ2+θ3+θ4\theta_{234} = \theta_2+\theta_3+\theta_4 — the hallmark of three parallel joint axes (compare the θ12\theta_{12} terms of the planar 2R arm). θ6\theta_6 does not appear at all: the last joint only rolls the gripper about its own axis. In practice nobody multiplies this out by hand — the three-step procedure plus a computer is the whole point.

Robot.fk matches fk_ur5e: True
<Figure size 1500x550 with 3 Axes>
Loading...

Key Takeaways — Section 5.8

  • The same three steps handle a real 6-DOF industrial arm; the only work is Step 1 (choosing frames) and Step 2 (writing the home transforms).

  • A robot model file (MJCF, URDF) gives the geometry, but you may re-assign frames to follow our convention — the gripper pose Te0\mathbf{T}_e^0 does not change.

  • Parallel joint axes show up as angle sums (θ23\theta_{23}, θ234\theta_{234}) in the closed form.

5.9 The FK Frame Animator and Chapter Summary

The FK Frame Animator (Ch5 FK Frame Animator.html, in the same folder as this notebook) shows all five examples of this chapter as a 3D model you can orbit with the mouse. It has two modes:

  • Joint motion: drag the joint sliders (or press Animate joints) and watch every frame ride on its link. The live Te0\mathbf{T}_e^0 is shown next to the closed-form position, so you can check your own derivations.

  • Step through product: the factors of Te0\mathbf{T}_e^0 are applied one at a time, each one moving the frame along or about its current axes — the animated version of the factors slider used in this notebook.

Run the cell below to open it in your web browser.

Chapter 5 Summary

  • An HTM TCA\mathbf{T}_C^A stores the pose of frame {C}\{C\} relative to {A}\{A\}; its columns are xCx_C, yCy_C, zCz_C and oCo_C expressed in {A}\{A\}.

  • Six basic HTMs (Transx,y,z\mathrm{Trans}_{x,y,z}, Rotx,y,z\mathrm{Rot}_{x,y,z}) build every link transform. A product of them is read left to right, each move about the current axes.

  • Forward kinematics in three steps:

    1. Frames: {0}\{0\} on the ground, {i}\{i\} on link ii with ziz_i along joint ii, {e}\{e\} at the gripper.

    2. Tii−1(θi)=Tii−1(0) Rotz,θi\mathbf{T}_i^{i-1}(\theta_i) = \mathbf{T}_i^{i-1}(0)\,\mathrm{Rot}_{z,\theta_i} (revolute) or Tii−1(0) Transz,θi\mathbf{T}_i^{i-1}(0)\,\mathrm{Trans}_{z,\theta_i} (prismatic).

    3. Te0=T10 T21⋯Tnn−1 Ten\mathbf{T}_e^0 = \mathbf{T}_1^0\,\mathbf{T}_2^1\cdots\mathbf{T}_n^{n-1}\,\mathbf{T}_e^n.

  • Forward kinematics always has exactly one answer. The reverse problem — find the joint variables that put the gripper at a desired pose — is inverse kinematics, and it is much harder.

ExampleJointsGripper position oeo_e
Planar 2RRR(L1c1+L2c12, L1s1+L2s12, 0)(L_1c_1 + L_2c_{12},\ L_1s_1 + L_2s_{12},\ 0)
Spherical wristRRRL (c1s2, s1s2, c2)L\,(c_1s_2,\ s_1s_2,\ c_2)
CylindricalRPP((a+d3)c1, (a+d3)s1, h+d2)\big((a+d_3)c_1,\ (a+d_3)s_1,\ h+d_2\big)
SCARARRPR(L1c1+L2c12, L1s1+L2s12, h−d3−de)(L_1c_1 + L_2c_{12},\ L_1s_1 + L_2s_{12},\ h-d_3-d_e)
UR5eRRRRRRsee Section 5.8