Foundations · Lesson 2 of 12

Spatial maths for robots: rotations and transforms

Rotation matrices, roll-pitch-yaw and gimbal lock, axis-angle, quaternions and SLERP, homogeneous transforms, and how a neural network should output a rotation.

intermediate50 minUpdated Fact-checked Free

What you will learn

  • Build 3D rotation matrices, check that they are valid and compose them in the right order
  • Convert between roll-pitch-yaw angles, axis-angle, quaternions and rotation matrices, and explain gimbal lock
  • Interpolate orientations with SLERP and rotate vectors with unit quaternions
  • Chain and invert homogeneous transforms to move points between world, base and camera frames
  • Choose a continuous rotation representation, such as the 6D form, for a neural network output

Before you start

  • Robots, AI and the SO-101 arm (lesson 1)
  • Vectors, matrix multiplication and the dot and cross products

After this lesson you can describe the orientation and position of any part of a robot, convert between the common rotation representations, and move points between frames such as world, robot base and camera. You will also know which rotation representation to use when a neural network has to predict an orientation, and why.

Frames, points and poses

A coordinate frame is an origin plus three perpendicular unit axes, x, y and z, arranged right-handed. A robot carries many frames: one fixed to the world, one at the base of the arm, one on each link, one on the gripper and one on each camera. Kinematics, perception and learning all keep asking the same question: given a point or direction in one frame, what are its coordinates in another?

The answer has two parts. A rotation says how the axes of one frame are turned relative to another. A translation says where its origin is. Together they form a pose. This lesson builds the rotation part first, then adds translation with homogeneous transforms.

Notation: RABR_{AB} is the orientation of frame B expressed in frame A. It maps a vector’s coordinates in B to its coordinates in A, vA=RAB vBv_A = R_{AB}\, v_B. Read the subscripts right to left: from B to A.

Rotation matrices

Two dimensions

Turning a 2D frame by an angle θ\theta gives

R(θ)=[cos⁡θ−sin⁡θsin⁡θcos⁡θ].R(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}.

The columns are the rotated x and y axes written in the original frame. That reading works in 3D too, and it is the quickest way to build or check a rotation matrix by hand.

Three dimensions

The three elementary rotations turn about one axis each:

Rx(α)=[1000cos⁡α−sin⁡α0sin⁡αcos⁡α],Ry(β)=[cos⁡β0sin⁡β010−sin⁡β0cos⁡β],Rz(γ)=[cos⁡γ−sin⁡γ0sin⁡γcos⁡γ0001].R_x(\alpha)=\begin{bmatrix}1&0&0\\0&\cos\alpha&-\sin\alpha\\0&\sin\alpha&\cos\alpha\end{bmatrix},\quad R_y(\beta)=\begin{bmatrix}\cos\beta&0&\sin\beta\\0&1&0\\-\sin\beta&0&\cos\beta\end{bmatrix},\quad R_z(\gamma)=\begin{bmatrix}\cos\gamma&-\sin\gamma&0\\\sin\gamma&\cos\gamma&0\\0&0&1\end{bmatrix}.

Every 3D rotation matrix has two properties, and together they define the group SO(3)SO(3):

  • Orthonormal: the columns are unit vectors at right angles to each other, so RTR=IR^\mathsf{T} R = I and the inverse is the transpose, R−1=RTR^{-1} = R^\mathsf{T}.
  • Determinant +1: the axes stay right-handed. An orthonormal matrix with determinant −1-1 is a reflection.

A 3×3 matrix has nine numbers but only three degrees of freedom. The six orthonormality constraints use up the rest. Numerical code drifts away from these constraints after many multiplications, so a long-running estimator re-orthonormalises its rotation from time to time.

Composing rotations

Rotations combine by matrix multiplication, and the order matters. If frame B is rotated relative to A, and C relative to B, then

RAC=RAB RBC.R_{AC} = R_{AB}\, R_{BC}.

The inner subscripts match and cancel. Matrix multiplication is not commutative: Rx(90∘)Rz(90∘)≠Rz(90∘)Rx(90∘)R_x(90^\circ) R_z(90^\circ) \neq R_z(90^\circ) R_x(90^\circ). Try it with a book. Turn it 90 degrees about a vertical axis and then about a horizontal one, and compare with the opposite order.

There are two ways to read a product such as Rz(γ)Ry(β)Rx(α)R_z(\gamma) R_y(\beta) R_x(\alpha). Read right to left, each rotation is about the fixed axes of the original frame: first x, then y, then z. Read left to right, each rotation is about the axes of the frame produced by the previous step (moving axes): first z, then the new y, then the newest x. Both readings give the same matrix.

Euler angles and roll, pitch, yaw

Three angles are the most compact way to write an orientation, and they are easy for people to read. An Euler angle convention picks three axes and an order. There are twelve valid sequences, and libraries disagree on defaults, so always state the convention.

Robotics software commonly uses roll-pitch-yaw (RPY) about fixed axes, which is the convention in URDF robot models: roll ϕ\phi about x, then pitch θ\theta about y, then yaw ψ\psi about z, all about the fixed parent axes. The matrix is

R=Rz(ψ) Ry(θ) Rx(ϕ).R = R_z(\psi)\, R_y(\theta)\, R_x(\phi).

SciPy calls this sequence "xyz" (lower case means fixed, or extrinsic, axes) in Rotation.from_euler. The test file checks the lesson code against it.

Gimbal lock

At pitch θ=±90∘\theta = \pm 90^\circ the roll and yaw axes line up, and one degree of freedom disappears. Multiplying out at θ=+90∘\theta = +90^\circ:

R=[0sin⁡(ϕ−ψ)cos⁡(ϕ−ψ)0cos⁡(ϕ−ψ)−sin⁡(ϕ−ψ)−100].R = \begin{bmatrix} 0 & \sin(\phi - \psi) & \cos(\phi - \psi) \\ 0 & \cos(\phi - \psi) & -\sin(\phi - \psi) \\ -1 & 0 & 0 \end{bmatrix}.

Only the difference ϕ−ψ\phi - \psi appears. Roll 10°, yaw 30° and roll 40°, yaw 60° give exactly the same matrix (the code checks this). Two consequences follow. Converting a matrix back to RPY has no unique answer at that pitch. And near it, a small change in orientation can need a large change in roll and yaw, which is a problem for any controller or learning algorithm that works directly on the angles.

Gimbal lock is a property of the three-angle description, not of the physical rotation. The fix is to use a representation without that singularity.

Axis-angle and Rodrigues’ formula

Any rotation is a turn by an angle θ\theta about some unit axis k^\hat{k} (Euler’s rotation theorem). Rodrigues’ formula builds the matrix:

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

[k^]×[\hat{k}]_\times is the skew-symmetric matrix with [k^]×v=k^×v[\hat{k}]_\times v = \hat{k} \times v. Check: with k^=(0,0,1)\hat{k} = (0, 0, 1) the formula gives Rz(θ)R_z(\theta).

Axis-angle has a clear geometric meaning, and the vector θk^\theta\hat{k} (a rotation vector) is a compact three-number form that SciPy calls rotvec. It is the natural way to describe a small orientation error. The axis is undefined at θ=0\theta = 0 and flips at θ=180∘\theta = 180^\circ, so it is still not a perfect global representation.

Unit quaternions

A quaternion has four numbers, q=(w,x,y,z)q = (w, x, y, z), with a scalar part ww and a vector part (x,y,z)(x, y, z). A rotation by θ\theta about unit axis k^\hat{k} is the unit quaternion

q=(cos⁡θ2, sin⁡θ2 k^).q = \left(\cos\tfrac{\theta}{2},\ \sin\tfrac{\theta}{2}\,\hat{k}\right).

The code in this lesson puts the scalar first. SciPy’s Rotation.as_quat() puts it last by default, and ROS messages store it last too. Mixing the two orders is one of the most common bugs in robot software.

Product and rotating a vector

Quaternions compose with the Hamilton product. For p=(pw,p)p = (p_w, \mathbf{p}) and q=(qw,q)q = (q_w, \mathbf{q}):

p⊗q=(pwqw−p⋅q,  pwq+qwp+p×q).p \otimes q = \left(p_w q_w - \mathbf{p}\cdot\mathbf{q},\ \ p_w \mathbf{q} + q_w \mathbf{p} + \mathbf{p} \times \mathbf{q}\right).

The product matches matrix multiplication: the matrix of p⊗qp \otimes q is R(p) R(q)R(p)\,R(q). To rotate a vector vv, write it as a quaternion with zero scalar part and sandwich it:

(0,v′)=q⊗(0,v)⊗q∗,q∗=(w,−x,−y,−z).(0, v') = q \otimes (0, v) \otimes q^{*}, \qquad q^{*} = (w, -x, -y, -z).

Example: 90° about z is q=(0.7071,0,0,0.7071)q = (0.7071, 0, 0, 0.7071), and it turns (1,0,0)(1, 0, 0) into (0,1,0)(0, 1, 0).

Double cover

The angle appears halved, so qq and −q-q give the same rotation: −q-q is a turn of θ+360∘\theta + 360^\circ about the same axis. Every rotation has two quaternions. Code that converts a matrix to a quaternion usually picks the one with w≥0w \ge 0. Code that compares or averages quaternions must allow for both signs.

Quaternions have no gimbal lock, need only four numbers with one constraint (∥q∥=1\|q\| = 1), and are cheap to renormalise. Robot software often stores orientation as a quaternion (a ROS Pose message does) and converts to a matrix when it needs to transform many points.

SLERP

A robot that moves its gripper from one orientation to another needs the orientations in between. Interpolating roll, pitch and yaw separately gives a curved, uneven path. Spherical linear interpolation (SLERP) moves along the shortest arc at constant angular speed:

slerp(q0,q1,t)=sin⁡((1−t)Ω)sin⁡Ω q0+sin⁡(tΩ)sin⁡Ω q1,cos⁡Ω=q0⋅q1,\mathrm{slerp}(q_0, q_1, t) = \frac{\sin\big((1 - t)\Omega\big)}{\sin\Omega}\, q_0 + \frac{\sin(t\Omega)}{\sin\Omega}\, q_1, \qquad \cos\Omega = q_0 \cdot q_1,

with tt from 0 to 1. The SIGGRAPH 1985 paper that proposed SLERP for animating rotations (listed under further reading) gives this form and the equivalent q0(q0−1q1)tq_0 (q_0^{-1} q_1)^t. If q0⋅q1<0q_0 \cdot q_1 < 0, negate q1q_1 first, or the interpolation takes the long way round. When the two are almost equal, sin⁡Ω\sin\Omega is close to zero, and the code falls back to normalised linear interpolation (nlerp): average the four components and renormalise.

Two line plots against the interpolation parameter t from 0 to 1 for a 160 degree rotation. Left: angle turned from the start. SLERP is a straight line from 0 to 160 degrees; nlerp is an S-shaped curve that starts slower and ends slower. Right: angular speed. SLERP is flat at 160 degrees per unit t; nlerp rises from about 113 to about 192 at the midpoint and falls back to about 113.
Interpolating a 160 degree rotation. SLERP turns at constant angular speed; nlerp follows the same path but is about 30 % slower at the ends and 20 % faster in the middle.

Both methods follow the same great-circle path; only the timing differs. For small rotations nlerp is close enough and cheaper. For long rotations that drive a robot directly, use SLERP.

Homogeneous transforms

A pose combines a rotation RR and a translation pp. Putting them in one 4×4 matrix lets you compose poses by multiplication:

TAB=[RABpAB0  0  01],[xA1]=TAB[xB1].T_{AB} = \begin{bmatrix} R_{AB} & p_{AB} \\ 0\ \ 0\ \ 0 & 1 \end{bmatrix}, \qquad \begin{bmatrix} x_A \\ 1 \end{bmatrix} = T_{AB} \begin{bmatrix} x_B \\ 1 \end{bmatrix}.

pABp_{AB} is the origin of B written in A. A point gets a 1 appended, so the translation applies; a direction gets a 0, so only the rotation applies. These matrices form the group SE(3)SE(3).

Chains of frames

Transforms chain like rotations, with matching inner subscripts:

TWC=TWB TBC,pW=TWC pC.T_{WC} = T_{WB}\, T_{BC}, \qquad p_W = T_{WC}\, p_C.
Perspective drawing of three coordinate frames with red x, teal y and blue z axes: a world frame W at the left, a robot base frame B on the floor rotated 90 degrees about z, and a camera frame C above the base whose z axis points forward toward an object point p. Curved arrows labelled T_WB, T_BC and a dashed T_WC connect the frames. A panel on the right lists T_WC = T_WB T_BC, p_W = T_WC p_C and T_CW = inverse of T_WC.
World, base and camera frames. Each arrow is the pose of one frame in another; chaining them gives the camera pose in the world, which maps a point seen by the camera into world coordinates.

Worked example. The robot base sits at (0.50,0.20,0)(0.50, 0.20, 0) m in the world, turned 90° about z. A camera is mounted 0.10 m in front of the base origin and 0.40 m up, looking forward along the base x axis. Cameras use z forward, x right and y down, so the camera axes, written in the base frame, are xC=(0,−1,0)x_C = (0, -1, 0), yC=(0,0,−1)y_C = (0, 0, -1), zC=(1,0,0)z_C = (1, 0, 0). Those are the columns of RBCR_{BC}, which equals RPY (−90∘,0,−90∘)(-90^\circ, 0, -90^\circ):

TWB=[0−100.501000.2000100001],TBC=[0010.10−10000−100.400001],TWC=[1000.500010.300−100.400001].T_{WB} = \begin{bmatrix} 0 & -1 & 0 & 0.50 \\ 1 & 0 & 0 & 0.20 \\ 0 & 0 & 1 & 0 \\ 0&0&0&1 \end{bmatrix},\quad T_{BC} = \begin{bmatrix} 0 & 0 & 1 & 0.10 \\ -1 & 0 & 0 & 0 \\ 0 & -1 & 0 & 0.40 \\ 0&0&0&1 \end{bmatrix},\quad T_{WC} = \begin{bmatrix} 1 & 0 & 0 & 0.50 \\ 0 & 0 & 1 & 0.30 \\ 0 & -1 & 0 & 0.40 \\ 0&0&0&1 \end{bmatrix}.

An object 0.60 m straight ahead of the camera is at pC=(0,0,0.60)p_C = (0, 0, 0.60). Then pW=TWC pC=(0.50,0.90,0.40)p_W = T_{WC}\,p_C = (0.50, 0.90, 0.40) m. In the world, the camera looks along +y, because the base x axis points along world y.

Inverse of a transform

To go the other way, invert. Because R−1=RTR^{-1} = R^\mathsf{T}, the inverse has a closed form, which is cheaper and more accurate than a general 4×4 inverse:

TAB−1=TBA=[RABT−RABT pAB0  0  01].T_{AB}^{-1} = T_{BA} = \begin{bmatrix} R_{AB}^\mathsf{T} & -R_{AB}^\mathsf{T}\, p_{AB} \\ 0\ \ 0\ \ 0 & 1 \end{bmatrix}.

The minus sign alone is not enough. The translation has to be re-expressed in the new frame, which is what the RTR^\mathsf{T} does. This step comes up constantly in camera geometry, for example when calibration gives the pose of a marker in the camera frame and you need the camera pose in the marker frame.

Rotations as network outputs

Learned policies and pose estimators often have to output an orientation. The obvious choices are three Euler angles or a four-number quaternion. Both cause trouble.

Consider yaw alone. Headings of +179.9∘+179.9^\circ and −179.9∘-179.9^\circ are 0.2° apart, but their target values are 359.8 apart. A network is a continuous function of its input, so to fit those targets it has to change its output steeply over a tiny range of inputs. Near that seam its predictions are poor, and training is slow. Quaternions have a similar seam. Picking w≥0w \ge 0 makes the map from rotation to quaternion jump for 180° rotations, where w=0w = 0 and the sign of the vector part flips.

A CVPR 2019 paper on the continuity of rotation representations (listed under further reading) formalised this. It defines a representation as continuous when the map from rotation to representation is continuous, and shows that every representation of 3D rotations in four or fewer real numbers is discontinuous somewhere. That includes Euler angles, axis-angle and quaternions. It then gives a continuous 6D representation: the first two columns of the rotation matrix. In its autoencoder experiments the discontinuous representations had mean errors up to 6 to 14 times higher than the continuous ones.

The 6D map back to a rotation is a Gram-Schmidt step. The network outputs two 3-vectors a1a_1 and a2a_2, and

b1=a1∥a1∥,b2=a2−(b1⋅a2) b1∥a2−(b1⋅a2) b1∥,b3=b1×b2,R=[ b1  b2  b3 ].b_1 = \frac{a_1}{\|a_1\|}, \qquad b_2 = \frac{a_2 - (b_1 \cdot a_2)\, b_1}{\| a_2 - (b_1 \cdot a_2)\, b_1 \|}, \qquad b_3 = b_1 \times b_2, \qquad R = [\, b_1\ \ b_2\ \ b_3 \,].

Any six numbers (except degenerate cases where a1a_1 and a2a_2 are parallel) give a valid rotation, so the network output never has to be repaired. The loss can then be computed on RR itself, for example as the squared difference to the target matrix. When the range of rotations is small, for example a gripper that never turns past ±90°, Euler angles work, because the seam is never reached.

The code

spatial.py collects everything in this lesson as small NumPy functions: rotx, roty, rotz, rpy_to_R and R_to_rpy, axis_angle_to_R, R_to_quat and quat_to_R, quat_mul, quat_rotate, slerp, make_T, transform_inverse, R_to_6d and sixd_to_R. The quaternion conversion picks the largest of w,x,y,zw, x, y, z first, so it stays accurate near 180°.

def transform_inverse(T: Array) -> Array:
    """Closed-form inverse: [R p; 0 1]^-1 = [R^T, -R^T p; 0 1]."""
    R, p = T[:3, :3], T[:3, 3]
    return make_T(R.T, -R.T @ p)


def sixd_to_R(v: Array) -> Array:
    a1, a2 = np.asarray(v[:3], dtype=float), np.asarray(v[3:], dtype=float)
    b1 = a1 / np.linalg.norm(a1)
    u2 = a2 - np.dot(b1, a2) * b1
    b2 = u2 / np.linalg.norm(u2)
    return np.column_stack((b1, b2, np.cross(b1, b2)))

test_spatial.py is a set of property tests on 200 seeded random rotations. It checks orthonormality and determinant, RPY against SciPy’s extrinsic "xyz", Rodrigues against SciPy’s rotation vectors, quaternion round trips up to sign, the product matching matrix multiplication, SLERP endpoints, unit norm and constant angular speed, the short-path rule, transform inverses, the camera example above, gimbal lock, and that the 6D vector stays continuous where yaw jumps by 360°.

Download spatial.py Download test_spatial.py
python spatial.py
python -m pytest -q    # 29 passed
Exercise

Locate an object seen by the camera

Use the frames from the worked example, with TWCT_{WC} as given.

  1. The camera reports an object at pC=(0.05,−0.02,0.50)p_C = (0.05, -0.02, 0.50) m. Where is it in the world frame?
  2. Where is the robot base origin, world point (0.50,0.20,0)(0.50, 0.20, 0), in the camera frame? Explain the sign of each coordinate.
Show solution
  1. pW=RWC pC+pWCp_W = R_{WC}\, p_C + p_{WC}. The rows of RWCR_{WC} are (1,0,0)(1, 0, 0), (0,0,1)(0, 0, 1) and (0,−1,0)(0, -1, 0), so RWC pC=(0.05,0.50,0.02)R_{WC}\,p_C = (0.05, 0.50, 0.02). Adding (0.50,0.30,0.40)(0.50, 0.30, 0.40) gives pW=(0.55,0.80,0.42)p_W = (0.55, 0.80, 0.42) m.
  2. Use the inverse: pC=RWCT(pW−pWC)p_C = R_{WC}^\mathsf{T}(p_W - p_{WC}). pW−pWC=(0,−0.10,−0.40)p_W - p_{WC} = (0, -0.10, -0.40), and RWCTR_{WC}^\mathsf{T} has rows (1,0,0)(1, 0, 0), (0,0,−1)(0, 0, -1) and (0,1,0)(0, 1, 0), so pC=(0,0.40,−0.10)p_C = (0, 0.40, -0.10) m. The base is 0.40 m along camera +y, which points down, and 0.10 m along camera −z, behind the image plane. That matches the mounting: 0.40 m up and 0.10 m forward of the base.

In code: transform_point(T_WC, [0.05, -0.02, 0.5]) and transform_point(transform_inverse(T_WC), [0.5, 0.2, 0.0]).

Exercise

Compose two quaternions

Let qzq_z be 90° about z and qxq_x be 90° about x.

  1. Write both quaternions.
  2. Compute qz⊗qxq_z \otimes q_x and use it to rotate v=(0,1,0)v = (0, 1, 0).
  3. Compute qx⊗qzq_x \otimes q_z, rotate the same vector, and explain why the result differs.
Show solution
  1. qz=(0.7071,0,0,0.7071)q_z = (0.7071, 0, 0, 0.7071) and qx=(0.7071,0.7071,0,0)q_x = (0.7071, 0.7071, 0, 0).
  2. qz⊗qx=(0.5,0.5,0.5,0.5)q_z \otimes q_x = (0.5, 0.5, 0.5, 0.5). This applies qxq_x first, then qzq_z, about fixed axes. qxq_x turns (0,1,0)(0, 1, 0) into (0,0,1)(0, 0, 1), and qzq_z leaves that unchanged, so the result is (0,0,1)(0, 0, 1).
  3. qx⊗qz=(0.5,0.5,−0.5,0.5)q_x \otimes q_z = (0.5, 0.5, -0.5, 0.5). Now qzq_z acts first: (0,1,0)(0, 1, 0) becomes (−1,0,0)(-1, 0, 0), and qxq_x leaves a vector on the x axis unchanged, so the result is (−1,0,0)(-1, 0, 0). Rotations do not commute, and the quaternion product keeps the same order as the matrix product RzRxR_z R_x versus RxRzR_x R_z.

Summary

  • A rotation matrix is orthonormal with determinant +1; its columns are the rotated axes, its inverse is its transpose, and rotations compose by multiplication in a fixed order.
  • Roll-pitch-yaw angles (URDF uses fixed-axis RzRyRxR_z R_y R_x) are compact and readable but hit gimbal lock at pitch ±90°, where only roll minus yaw is defined.
  • Axis-angle and Rodrigues’ formula describe a single turn about an axis. Unit quaternions store a rotation in four numbers without gimbal lock, with qq and −q-q equal.
  • SLERP interpolates orientations along the shortest arc at constant speed; take the short path by flipping the sign when q0⋅q1<0q_0 \cdot q_1 < 0.
  • Homogeneous transforms chain with matching subscripts, TWC=TWBTBCT_{WC} = T_{WB} T_{BC}, and invert in closed form, [RT,−RTp][R^\mathsf{T}, -R^\mathsf{T} p].
  • Every rotation representation with four or fewer numbers is discontinuous somewhere. A network that must output any rotation should predict the 6D form (two matrix columns) and map it back with Gram-Schmidt.

Check your understanding

Pick an answer for each question, then check. Answers stay hidden until you submit.

1. Which property does every rotation matrix R have?
2. With roll-pitch-yaw angles R = Rz(yaw) Ry(pitch) Rx(roll), what happens at pitch = 90 degrees?
3. The quaternions q and -q describe...
4. T_AB is the pose of frame B in frame A, with rotation R and translation p. What is the translation part of T_AB inverse?
5. Why is a raw yaw angle in degrees a poor output for a network that must predict any heading?

Further reading

Found a mistake or something unclear?Report a correction.