3. Rigid Body Pose
The first step in describing any robot is to say where each link is and which way it is pointing. This chapter builds the mathematical vocabulary: reference frames, rotation matrices, and their alternative parameterisations. We will see that the same \(3\times3\) matrix \(\mathbf{R}\) serves three distinct roles — as a description of orientation, as a change-of-coordinates map, and as an active rotation operator. Keeping those roles separate avoids confusion when composing transforms along a kinematic chain.
1 Frames and position
Consider a fixed reference frame \(O\text{-}xyz\) and a body frame \(O'\text{-}x'y'z'\) rigidly attached to a moving body. The pose of the body is fully described by:
- the position of the body-frame origin \(O'\), expressed as the vector \(\overrightarrow{OO'}\) on the reference axes, and
- the orientation of the body frame relative to the reference frame.
Expressing the origin offset on the reference axes,
\[ \overrightarrow{OO'} = \begin{bmatrix} o'_x \\ o'_y \\ o'_z \end{bmatrix}, \qquad \overrightarrow{O'} = o'_x\,\hat{x} + o'_y\,\hat{y} + o'_z\,\hat{z}. \]
The reference unit vectors are \(\hat{x}=[1,0,0]^{\mathsf{T}}\), \(\hat{y}=[0,1,0]^{\mathsf{T}}\), \(\hat{z}=[0,0,1]^{\mathsf{T}}\).
2 The rotation matrix
Let \(\hat{x}',\hat{y}',\hat{z}'\) be the unit vectors of the body frame \(O'\text{-}x'y'z'\), written with respect to \(O\text{-}xyz\). Each is a linear combination of the reference axes:
\[ \begin{aligned} \hat{x}' &= x'_x\,\hat{x} + x'_y\,\hat{y} + x'_z\,\hat{z},\\ \hat{y}' &= y'_x\,\hat{x} + y'_y\,\hat{y} + y'_z\,\hat{z},\\ \hat{z}' &= z'_x\,\hat{x} + z'_y\,\hat{y} + z'_z\,\hat{z}. \end{aligned} \]
Stacking the body axes as columns gives the rotation matrix
\[ \mathbf{R}= \begin{bmatrix} \hat{x}' & \hat{y}' & \hat{z}' \end{bmatrix} = \begin{bmatrix} x'_x & y'_x & z'_x\\ x'_y & y'_y & z'_y\\ x'_z & y'_z & z'_z \end{bmatrix}. \]
Each entry is a direction cosine — the dot product between a body axis and a reference axis. For example, the first column collects \(\hat{x}'^{\mathsf{T}}\hat{x}\), \(\hat{x}'^{\mathsf{T}}\hat{y}\), \(\hat{x}'^{\mathsf{T}}\hat{z}\).
3 Orthonormality
Because the body axes are mutually orthogonal unit vectors,
\[ \hat{x}'^{\mathsf{T}}\hat{y}' = 0,\quad \hat{y}'^{\mathsf{T}}\hat{z}' = 0,\quad \hat{z}'^{\mathsf{T}}\hat{x}' = 0 \qquad\text{(orthogonality)}, \] \[ \hat{x}'^{\mathsf{T}}\hat{x}' = 1,\quad \hat{y}'^{\mathsf{T}}\hat{y}' = 1,\quad \hat{z}'^{\mathsf{T}}\hat{z}' = 1 \qquad\text{(unit length)}. \]
These six constraints mean that \(\mathbf{R}\) belongs to the special orthogonal group \(\mathrm{SO}(3)\):
\[ \mathbf{R}^{\mathsf{T}}\mathbf{R}= \mathbf{I}, \qquad \det\mathbf{R}= +1, \qquad\Longrightarrow\qquad \boxed{\;\mathbf{R}^{\mathsf{T}}= \mathbf{R}^{-1}\;} \]
The transpose equals the inverse — a fact used constantly when reversing a transformation. The 9 elements of \(\mathbf{R}\) carry only 3 independent degrees of freedom (the remaining 6 are fixed by the orthonormality constraints).
4 Elementary rotations
A rotation by angle \(\alpha\) about the \(z\)-axis (\(\hat{z}=\hat{z}'\)) sends the reference axes to
\[ \hat{x}' = \cos\alpha\,\hat{x} + \sin\alpha\,\hat{y}, \qquad \hat{y}' = -\sin\alpha\,\hat{x} + \cos\alpha\,\hat{y}, \]
giving the three elementary rotation matrices:
\[ \mathbf{R}_z(\alpha)= \begin{bmatrix} \cos\alpha & -\sin\alpha & 0\\ \sin\alpha & \cos\alpha & 0\\ 0 & 0 & 1 \end{bmatrix}, \]
\[ \mathbf{R}_y(\beta)= \begin{bmatrix} \cos\beta & 0 & \sin\beta\\ 0 & 1 & 0\\ -\sin\beta & 0 & \cos\beta \end{bmatrix}, \qquad \mathbf{R}_x(\gamma)= \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\gamma & -\sin\gamma\\ 0 & \sin\gamma & \cos\gamma \end{bmatrix}. \]
A negative rotation is simply the transpose,
\[ \mathbf{R}_k(-\theta) = \mathbf{R}_k^{\mathsf{T}}(\theta), \qquad k \in \{x,y,z\}, \]
consistent with orthogonality.
5 Vector representation across frames
The rotation matrix serves three equivalent roles simultaneously:
Orientation description. \(\mathbf{R}\) describes the orientation of frame \(O'\) relative to frame \(O\) by storing the body-frame axes as its columns.
Change of coordinates (transformation matrix). A point \(P\) has coordinates \(\boldsymbol{p}' = [p'_x, p'_y, p'_z]^{\mathsf{T}}\) in the body frame. Expanding the same physical vector on the reference axes gives the change of representation:
\[ \boxed{\;\boldsymbol{p}= \mathbf{R}\,\boldsymbol{p}'\;} \]
In 2D this reads \(\begin{bmatrix} p_x \\ p_y \end{bmatrix} = \begin{bmatrix} \cos\alpha & -\sin\alpha \\ \sin\alpha & \cos\alpha \end{bmatrix} \begin{bmatrix} p'_x \\ p'_y \end{bmatrix}\).
In this role, \(\mathbf{R}\) is the transformation matrix that maps coordinates from the body frame to the reference frame (passive interpretation: the point does not move; only its description changes).
- Active rotation. The same matrix rotates a vector within a fixed frame by angle \(\alpha\). Using the angle-sum identities, a point at angle \(\beta\) with magnitude \(|p|\) moves to angle \(\alpha+\beta\), which is exactly the action of \(\mathbf{R}_z(\alpha)\).
6 Composition of rotations
Given three frames \(0\), \(1\), \(2\), a vector in frame \(2\) is brought into frame \(0\) by composing the intermediate rotations:
\[ \boldsymbol{p}^0 = \mathbf{R}^0_2\,\boldsymbol{p}^2, \qquad \mathbf{R}^0_2 = \mathbf{R}^0_1\,\mathbf{R}^1_2. \]
This follows directly from \(\boldsymbol{p}^0 = \mathbf{R}^0_1\boldsymbol{p}^1 = \mathbf{R}^0_1(\mathbf{R}^1_2\boldsymbol{p}^2)\).
Pre- vs. post-multiplication. When rotations are applied about the axes of the current (rotated) frame, they are post-multiplied:
\[ \mathbf{R}= \mathbf{R}_1 \cdot \mathbf{R}_2 \cdot \mathbf{R}_3 \quad \text{(body-fixed axes).} \]
When rotations are applied about fixed reference axes, they are pre-multiplied in the opposite order.
Two useful identities follow from orthogonality:
\[ (\mathbf{R}^0_1)^{-1} = \mathbf{R}^1_0 = (\mathbf{R}^0_1)^{\mathsf{T}}, \] \[ \mathbf{R}^0_1\,\mathbf{R}^1_2\,\mathbf{R}^2_1\,\mathbf{R}^1_0 = \mathbf{I}. \]
7 Euler angles
Rotation matrices have 9 elements and 6 constraints, so only 3 degrees of freedom are needed to describe orientation. Euler angles parameterise orientation by three successive rotations. There are \(3^3 = 27\) possible axis combinations. However, two consecutive rotations must not be made around the same axis (the second would merely add to the first, using up a degree of freedom for nothing). This constraint eliminates 15 sets and leaves exactly 12 valid Euler angle conventions.
7.1 ZYZ convention
The most common set in robotics is the ZYZ convention, with \(\boldsymbol{\phi}=[\varphi,\vartheta,\psi]^{\mathsf{T}}\):
- Rotate by \(\varphi\) about \(z\) → \(R_z(\varphi)\)
- Rotate by \(\vartheta\) about the current \(y'\) → \(R_{y'}(\vartheta)\)
- Rotate by \(\psi\) about the current \(z''\) → \(R_{z''}(\psi)\)
\[ \mathbf{R}(\boldsymbol\phi) = \mathbf{R}_z(\varphi)\,\mathbf{R}_y(\vartheta)\,\mathbf{R}_z(\psi). \]
Carrying out the product (writing \(c_\bullet=\cos\), \(s_\bullet=\sin\)):
\[ \mathbf{R}= \begin{bmatrix} c_\varphi c_\vartheta c_\psi - s_\varphi s_\psi & -c_\varphi c_\vartheta s_\psi - s_\varphi c_\psi & c_\varphi s_\vartheta \\[2pt] s_\varphi c_\vartheta c_\psi + c_\varphi s_\psi & -s_\varphi c_\vartheta s_\psi + c_\varphi c_\psi & s_\varphi s_\vartheta \\[2pt] -s_\vartheta c_\psi & s_\vartheta s_\psi & c_\vartheta \end{bmatrix}. \]
Inverse problem (extracting the angles). Given \(\mathbf{R}\), read off the angles as:
\[ \vartheta = \operatorname{atan2}\!\left(\sqrt{r_{13}^2 + r_{23}^2},\; r_{33}\right), \]
with \(\varphi\) and \(\psi\) recovered from the remaining entries. The two-argument arctangent is used throughout to resolve the correct quadrant. Note that two solutions exist (corresponding to \(+\vartheta\) and \(-\vartheta\)), analogous to the elbow-up/elbow-down ambiguity in inverse kinematics.
7.2 Roll–Pitch–Yaw (RPY)
The RPY convention uses rotations about fixed (world) axes, with \(\boldsymbol\phi=[\varphi,\vartheta,\psi]^{\mathsf{T}}\) denoting roll, pitch, and yaw:
\[ \mathbf{R}(\boldsymbol\phi) = \mathbf{R}_z(\psi)\,\mathbf{R}_y(\vartheta)\,\mathbf{R}_x(\varphi). \]
Because the axes are fixed, later rotations are pre-multiplied on the left — note the reversed order compared to body-fixed ZYZ.
8 Axis and angle
Any rotation can be specified by a unit axis \(\hat{\boldsymbol n}=[n_x,n_y,n_z]^{\mathsf{T}}\) and an angle \(\theta\). The corresponding rotation matrix is obtained by aligning \(\hat{\boldsymbol n}\) with \(\hat z\), rotating by \(\theta\), then undoing the alignment:
\[ \mathbf{R}(\hat{\boldsymbol n},\theta) = \mathbf{R}_z(\alpha)\,\mathbf{R}_y(\beta)\,\mathbf{R}_z(\theta)\,\mathbf{R}_y(-\beta)\,\mathbf{R}_z(-\alpha), \]
where the alignment angles satisfy
\[ \sin\alpha = \frac{n_y}{\sqrt{n_x^2+n_y^2}},\quad \cos\alpha = \frac{n_x}{\sqrt{n_x^2+n_y^2}},\quad \sin\beta = \sqrt{n_x^2+n_y^2},\quad \cos\beta = n_z . \]
A useful symmetry: reversing both the axis and the angle gives the same rotation,
\[ \mathbf{R}(-\theta,-\hat{\boldsymbol n}) = \mathbf{R}(\theta,\hat{\boldsymbol n}). \]
Singularity. The inverse problem (extracting \(\theta\) and \(\hat{\boldsymbol n}\) from \(\mathbf{R}\)) breaks down when \(\sin\theta = 0\), i.e. at \(\theta = 0\) or \(\theta = \pi\). At \(\theta = 0\) the rotation is the identity and the axis is undefined; at \(\theta = \pi\) the direction of the axis becomes ambiguous. If \(\sin\theta = 0\), there are problems! Unit quaternions (below) resolve both cases without singularity.
9 Unit quaternions
A unit quaternion \(\mathcal{Q} = \{\eta,\;\boldsymbol\varepsilon\}\) encodes the axis/angle pair without the singularities:
\[ \eta = \cos\!\frac{\theta}{2}\ \text{(scalar part)}, \qquad \boldsymbol\varepsilon = \sin\!\frac{\theta}{2}\,\hat{\boldsymbol n} =\begin{bmatrix}\varepsilon_x\\ \varepsilon_y\\ \varepsilon_z\end{bmatrix} \ \text{(vector part)}. \]
The unit-norm constraint follows directly from \(\cos^2+\sin^2=1\):
\[ \eta^2 + \varepsilon_x^2 + \varepsilon_y^2 + \varepsilon_z^2 = 1 . \]
Advantages over Euler angles: no representation singularities (gimbal lock), numerically stable interpolation (spherical linear interpolation, SLERP), and efficient composition (quaternion multiplication replaces matrix multiplication).
10 Homogeneous transformations
10.1 Combining rotation and translation
Position changes by both a rotation and a translation. For a point known in frame \(1\),
\[ \boldsymbol{p}^0 = \overrightarrow{O'^{\,0}_1} + \mathbf{R}^0_1\,\boldsymbol{p}^1. \]
Augmenting the coordinate vector with a trailing \(1\), \(\tilde{\boldsymbol{p}} = [\boldsymbol{p}^{\mathsf{T}}, 1]^{\mathsf{T}}\in\mathbb{R}^4\), lets this be written as a single \(4\times4\) homogeneous transformation:
\[ \mathbf{A}^0_1 = \begin{bmatrix} \mathbf{R}^0_1 & \overrightarrow{O'^{\,0}_1}\\[2pt] \mathbf{0}^{\mathsf{T}}& 1 \end{bmatrix}, \qquad \tilde{\boldsymbol{p}}^0 = \mathbf{A}^0_1\,\tilde{\boldsymbol{p}}^1 . \]
The bottom row enforces \(1=1\), ensuring the augmented form is consistent.
10.2 Inverse transformation
Unlike rotation matrices, \(\mathbf{A}^{-1}\neq\mathbf{A}^{\mathsf{T}}\) in general. Inverting \(\boldsymbol{p}^0 = \overrightarrow{O'^{\,0}_1} + \mathbf{R}^0_1\boldsymbol{p}^1\) gives
\[ \mathbf{A}^1_0 = (\mathbf{A}^0_1)^{-1} = \begin{bmatrix} \mathbf{R}^1_0 & -\mathbf{R}^1_0\,\overrightarrow{O'^{\,0}_1}\\[2pt] \mathbf{0}^{\mathsf{T}}& 1 \end{bmatrix}. \]
10.3 Composition along a chain
Transformations compose by matrix product, exactly as rotation matrices do:
\[ \tilde{\boldsymbol{p}}^0 = \mathbf{A}^0_1\,\mathbf{A}^1_2\,\mathbf{A}^2_3\cdots\mathbf{A}^{n-1}_n\,\tilde{\boldsymbol{p}}^n . \]
This is the algebraic backbone of open-chain kinematics and motivates the Denavit–Hartenberg convention in the next chapter.