Homogeneous transforms and rigid-body motion

A pose combines position and orientation, and robots need to chain poses constantly: the camera relative to the wrist, the wrist relative to the base, the base relative to the map, the object relative to the camera. Doing this with separate rotation matrices and translation vectors works, but the bookkeeping quickly becomes error-prone. The homogeneous transformation matrix packs a rotation and a translation into a single 4 × 4 matrix, so that changing frames, composing poses and inverting them all become matrix operations. This lesson develops homogeneous transforms, the group SE(3) they form, and the habits that make them reliable in real code.

Combining rotation and translation

Suppose frame B is both rotated and translated relative to frame A. A point P has coordinates \( {}^{B}\mathbf{p} \) in B. To find its coordinates in A, we first rotate them into A's orientation and then add the position of B's origin:

This is an affine map, not a linear one, because of the added translation. That makes chaining awkward: going from C to B to A gives \( {}^{A}R_{B}({}^{B}R_{C}\,{}^{C}\mathbf{p} + {}^{B}\mathbf{p}_{C}) + {}^{A}\mathbf{p}_{B} \), and every additional frame adds another nested layer. The trick of homogeneous coordinates removes the problem by appending a fourth coordinate equal to 1 to every point.

The homogeneous transformation matrix

Write a point as \( (x, y, z, 1) \) and define the 4 × 4 matrix:

Then the frame change becomes a single matrix product, \( {}^{A}\tilde{\mathbf{p}} = {}^{A}T_{B}\,{}^{B}\tilde{\mathbf{p}} \), where the tilde marks homogeneous coordinates. Multiplying out confirms that the top three rows reproduce the affine formula and the last row keeps the 1. Like a rotation matrix, the transform has several readings: it describes the pose of frame B relative to A (the columns are B's axes and B's origin, all expressed in A); it converts coordinates from B to A; and it can act as an operator that moves a body within one frame.

Free vectors such as directions and velocities are written with a fourth coordinate of 0 rather than 1. Then the translation column is multiplied by zero and only the rotation acts, exactly as lesson 1 required: displacements are unaffected by moving the origin. This 0-or-1 convention lets a single matrix handle both points and vectors correctly.

Composition: chaining frames

The great advantage of homogeneous transforms is that chaining frames is just multiplication, with frame labels canceling:

The block form shows what happens: rotations multiply, and the translation of C is first rotated into A's orientation and then added to B's offset. A whole robot arm, as lessons 5 and 6 show, is described by a chain of such transforms from the base through each link to the tool: \( {}^{0}T_{6} = {}^{0}T_{1}\,{}^{1}T_{2} \cdots {}^{5}T_{6} \). A mobile manipulator adds the robot's pose in the map at the front of the chain, and a camera adds its mounting transform at the end.

As with rotations, the order of multiplication encodes whether a motion is described in the fixed frame or the moving frame. Post-multiplying, \( T\,T_{\text{motion}} \), applies the motion relative to the current, moving frame: 'move 10 cm along the tool's own z-axis'. Pre-multiplying, \( T_{\text{motion}}\,T \), applies it relative to the fixed frame: 'move 10 cm up in the world'. Industrial robot pendants offer exactly these two jog modes, usually called tool and world (or base) coordinates.

The inverse transform

Inverting a transform reverses the direction of description: \( {}^{B}T_{A} = ({}^{A}T_{B})^{-1} \). Because the rotation part is orthonormal, the inverse has a closed form that is cheaper and more accurate than a general 4 × 4 inverse:

The translation part is not simply \( -\mathbf{p} \). It must be rotated, because the offset from B back to A has to be expressed in B's axes. Forgetting this rotation is the single most common transform bug; it produces results that look right when the rotation is close to the identity and wrong otherwise, so it can survive simple tests.

SE(3): the group of rigid-body motions

Homogeneous transforms with a rotation in SO(3) and any translation form the special Euclidean group SE(3). Like SO(3), it is closed under multiplication and inversion, and every element preserves distances and angles: a rigid-body motion moves an object without stretching, shearing or mirroring it. An element of SE(3) has six degrees of freedom, three for rotation and three for translation, even though the matrix has sixteen entries. The planar version, SE(2), uses 3 × 3 matrices with a 2-D rotation and a 2-D translation and is the natural language for mobile robots on flat ground:

Seeing poses as elements of a group is more than tidy mathematics. It explains why poses should not be averaged or interpolated component by component, why errors in pose estimation are best described in a local tangent space, and why the exponential map of lesson 7 is the right tool for integrating velocities into poses.

Relative poses and the hand-eye problem

Many practical calculations ask for the transform between two frames that are both known relative to a third. If a motion-capture system reports the poses of a robot base and a target in its own frame, \( {}^{M}T_{B} \) and \( {}^{M}T_{G} \), the target relative to the robot is \( {}^{B}T_{G} = ({}^{M}T_{B})^{-1}\,{}^{M}T_{G} \). The pattern is always the same: invert the transform that has the wanted frame on the wrong side, then multiply so that the labels cancel.

A famous version is hand-eye calibration. A camera is mounted on a robot's wrist, and we need the fixed but unknown transform X from the wrist to the camera. Moving the robot between poses and observing a fixed calibration target from each gives equations of the form \( A X = X B \), where A is the known wrist motion from the robot's forward kinematics and B is the camera motion measured from the target. Several motions with different rotation axes determine X. Libraries such as OpenCV's calibrateHandEye solve it; the quality of the result depends on choosing varied, well-separated poses.

Transforms in code

In software, transforms are usually stored as 4 × 4 arrays, or as a quaternion plus a translation vector in messages such as geometry_msgs/TransformStamped. The listing builds the transforms from the camera example, composes and inverts them, and checks the closed-form inverse against NumPy's general inverse.

import numpy as np
np.set_printoptions(precision=3, suppress=True)
def Rz(t): c, s = np.cos(t), np.sin(t); return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def Rx(t): c, s = np.cos(t), np.sin(t); return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def T(R, p): M = np.eye(4); M[:3,:3] = R; M[:3,3] = p; return M
def inv(M):
    R, p = M[:3,:3], M[:3,3]
    return T(R.T, -R.T @ p)

T_B_W = T(Rz(np.radians(45)), [0.4, 0.0, 0.6])       # wrist in base
T_W_C = T(Rx(np.pi), [0.05, 0.0, 0.10])              # camera in wrist
p_C = np.array([0.02, -0.01, 0.30, 1.0])             # object seen by camera
p_B = T_B_W @ T_W_C @ p_C
print("object in base:", p_B[:3])
print("closed-form inverse matches:", np.allclose(inv(T_B_W), np.linalg.inv(T_B_W)))
print("naive inverse translation:  ", -T_B_W[:3,3])
print("correct inverse translation:", inv(T_B_W)[:3,3])

object in base: [0.442 0.057 0.4  ]
closed-form inverse matches: True
naive inverse translation:   [-0.4 -0.  -0.6]
correct inverse translation: [-0.283  0.283 -0.6  ]

Good habits

Name every transform with both frames, such as T_base_cam, and write the composition so the inner names match. Store frame names with data in messages and logs, as tf2 does with frame_id and child_frame_id. Check transforms visually in RViz by displaying axis triads, because a flipped axis is obvious in a picture and invisible in a table of numbers. Test round trips: transforming a point forward and back with the inverse should reproduce it to machine precision. Finally, be careful when reading other people's conventions. Some libraries store the transform from parent to child, others from child to parent, and the words 'from' and 'to' are used inconsistently. ROS tf2 lookups, for example, take a target frame and a source frame and return the transform that converts data from the source frame into the target frame.