Section 4.4: Rigid transforms, homogeneous coordinates, SE(3)

A Careful Control Loop

This section develops the technical contract for rigid transforms, homogeneous coordinates, and the Lie group SE(3). First we distinguish rotation, translation, and frame labels. Then we show how homogeneous coordinates make composition a single matrix multiplication. Finally we test one transform chain numerically so a reader can see exactly where a pose bug would enter a robot pipeline.

The key question is practical: if a camera sees a block at one coordinate, how does the robot compute the coordinate that its controller can actually use?

Theory

A rigid transform preserves distances and angles. In 3D, it combines a rotation $R \in SO(3)$ and a translation $t \in \mathbb{R}^3$. For a point $p_B$ expressed in frame $B$, the same physical point expressed in frame $A$ is

$$p_A = R_{AB}p_B + t_{AB}.$$

The subscript convention matters. Here $R_{AB}$ rotates coordinates from frame $B$ into frame $A$, and $t_{AB}$ is the origin of frame $B$ expressed in frame $A$. The transform is valid only when both frames are rigid, the units match, and the timestamp is appropriate for the motion being controlled.

Homogeneous coordinates append a final 1 to a point so rotation and translation can be composed by one matrix:

$$T_{AB} = \begin{bmatrix} R_{AB} & t_{AB} \\ 0\ 0\ 0 & 1 \end{bmatrix}, \qquad \bar p_A = T_{AB}\bar p_B.$$

The useful payoff is composition. If a camera frame $C$ is attached to a wrist frame $W$, and the wrist is attached to the robot base frame $B$, then $T_{BC} = T_{BW}T_{WC}$. Matrix order encodes the path through the frame graph. Reversing the order usually produces a plausible-looking number with the wrong physical meaning.

Worked Example

Consider a wrist camera that detects a block at $(0.30, 0.10, 0.20)$ meters in the camera frame. The transform below converts that point into the robot base frame. Code Fragment 4.4.1 keeps the numbers small enough to check by hand: the camera is offset 0.50 meters forward and 0.25 meters upward from the wrist, and the wrist is 1.00 meter above the base.

# Compose two SE(3) transforms and apply them to one detected point.
# The final identity check catches inverted transform order or bad inverses.
# All distances are meters and all matrices use column-vector convention.
import numpy as np

def make_transform(rotation, translation):
    transform = np.eye(4)
    transform[:3, :3] = np.array(rotation, dtype=float)
    transform[:3, 3] = np.array(translation, dtype=float)
    return transform

base_from_wrist = make_transform(np.eye(3), [0.0, 0.0, 1.0])
wrist_from_camera = make_transform(np.eye(3), [0.5, 0.0, 0.25])
base_from_camera = base_from_wrist @ wrist_from_camera

point_camera = np.array([0.30, 0.10, 0.20, 1.0])
point_base = base_from_camera @ point_camera
round_trip = np.linalg.inv(base_from_camera) @ point_base

print("base point:", point_base[:3].round(3).tolist())
print("round trip:", round_trip[:3].round(3).tolist())

Expected output: the base-frame point is shifted by the two translations, while the round-trip output returns to the original camera measurement. If the round trip fails, debug transform order, units, and frame labels before changing perception or control code.

SE(3) bugs arise when rotation, translation, and composition order are mixed. Check inverse, composition, and point-transformation by hand before trusting a transform tree.