Coordinate frames and rigid transforms

You have spent a career making sure tensors have the right shape, dtype, and device before a kernel touches them. Robotics has an equivalent invariant, and nothing enforces it: every 3D quantity in your stack — a grasp point, a joint axis, a velocity command — is a handful of numbers expressed in some coordinate frame, and the frame is recorded nowhere in the array. The array [0.42, -0.03, 0.11] in the camera frame and the identical array in the arm base frame are different physical points roughly 60 cm apart, and numpy will happily add them together. This lesson gives you the discipline — a naming convention, the underlying group theory, and cheap numerical checks — that turns silent frame corruption into loud, early failure.

Foundations: Frames, Poses, and the Geometry of Rotation

Before manipulating matrices, we must rigorously define the objects they represent. A Pose is the complete state of a rigid body in space, consisting of a position vector $p \in \mathbb{R}^3$ and an orientation $R \in SO(3)$. A Transform $T_{AB}$ is the mathematical operator that maps coordinates from frame $B$ to frame $A$. While often used interchangeably, the distinction is critical: $T_{AB}$ is the pose of frame $B$ expressed in frame $A$. The translation column of $T_{AB}$ is the position of $B$'s origin in $A$, and the rotation block is the orientation of $B$'s axes in $A$.

All frames in this course are right-handed. This means the cross product of the first two axes yields the third: $\hat{x} \times \hat{y} = \hat{z}$. This convention ensures that rotation matrices have a determinant of $+1$. The Robot Convention (used for arm links) typically aligns $\hat{x}$ forward, $\hat{y}$ left, and $\hat{z}$ up. The Optical Convention (used for cameras) aligns $\hat{z}$ forward along the optical axis, $\hat{x}$ right, and $\hat{y}$ down. Note that the optical frame is often left-handed in image pixel space, but the 3D camera body frame remains right-handed. Confusing these conventions is the primary source of sign errors in extrinsic calibration.

To understand how rotation matrices are constructed, let frame $B$ be oriented by a positive right-hand-rule rotation $\theta$ about the $+\hat{y}$ axis relative to frame $A$. We derive $R_{AB}=R_y(\theta)$ by expressing $B$'s basis axes in $A$ coordinates. Positive rotation about $+\hat y$ moves $\hat{x}_B$ toward $-\hat{z}_A$, so its coordinates are $(\cos\theta, 0, -\sin\theta)$; it moves $\hat{z}_B$ toward $+\hat{x}_A$, giving $(\sin\theta, 0, \cos\theta)$. The $\hat{y}$ axis is unchanged. Placing those three $B$ axes as columns yields the standard form. Calling this matrix 'active' or 'passive' without naming the source and destination frames invites a sign mistake, so this course always states the mapping instead: $p_A=R_{AB}p_B$.

The set of all such rotation matrices forms the Lie Group $SO(3)$. Its associated Lie Algebra $\mathfrak{so}(3)$ is the tangent space at the identity, consisting of all skew-symmetric matrices $\hat{\omega}$ where $\hat{\omega}^\top = -\hat{\omega}$. For any vector $\omega \in \mathbb{R}^3$, the skew-symmetric matrix $\hat{\omega}$ is defined such that $\hat{\omega}v = \omega \times v$ for any vector $v$. The exponential map $\exp: \mathfrak{so}(3) \to SO(3)$ connects the algebra to the group: $R = \exp(\hat{\omega})$. This is the rigorous bridge between axis-angle vectors (which live in the algebra) and rotation matrices (which live in the group).

A critical numerical issue is drift. After many matrix multiplications, floating-point errors cause the rotation block $R$ to deviate from orthogonality ($R^\top R \neq I$). This drift is not just a theoretical concern; it accumulates linearly with the number of operations. To restore validity, we project $R$ back onto $SO(3)$ using the Singular Value Decomposition (SVD). If $R = U \Sigma V^\top$, the closest orthogonal matrix is $R_{proj} = U V^\top$. This step is mandatory in any long-running kinematics chain.

Worked Example: Deriving and Inverting a Rigid Transform

Let us compute the transform $T$ for a camera mounted with a $45^\circ$ pitch and a translation $t = [0.1, 0.2, 0.3]^\top$ meters. First, we construct $R_y(45^\circ)$. Since $\cos(45^\circ) = \sin(45^\circ) = \frac{\sqrt{2}}{2} \approx 0.7071$, the rotation matrix is:

The full homogeneous transform is $T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}$. To find the inverse $T^{-1}$, we use the derived formula $T^{-1} = \begin{bmatrix} R^\top & -R^\top t \\ 0 & 1 \end{bmatrix}$. The key step is computing $-R^\top t$. We perform the matrix-vector multiplication $R^\top t$:

Thus, $T^{-1} = \begin{bmatrix} 0.7071 & 0 & -0.7071 & 0.14142 \\ 0 & 1 & 0 & -0.2 \\ 0.7071 & 0 & 0.7071 & -0.28284 \\ 0 & 0 & 0 & 1 \end{bmatrix}$. We verify this by checking that $T T^{-1} = I$. The top-left block is $R R^\top = I$. The top-right block is $R(-R^\top t) + t = -t + t = 0$. The bottom row is trivially $[0,0,0,1]$. This explicit block multiplication confirms that the inverse correctly undoes both the rotation and the translation, a step often skipped in favor of generic np.linalg.inv which is slower and less numerically stable for rigid bodies.

The frame zoo: why every robotics bug is secretly a frames bug

Walk around the smallest useful manipulation cell — a WidowX AI arm bolted to a table, a RealSense-class RGB-D camera on a rigid mount, one object to pick up — and count coordinate frames:

• world / table — the fixed reference everything hangs from; often coincident with the arm base until a second robot or moving cart earns it a name of its own.
• base_link — the arm's root frame beneath the base yaw joint (J0). Cartesian goals, workspace limits, and most dataset action conventions live here.
• one link frame per joint — the URDF of the 6-DOF WidowX AI defines a chain of link frames (shoulder, upper arm, forearm, wrist segments, gripper carriage), whose mutual transforms are functions of the joint angles from the previous lesson and change every 2 ms controller cycle.
• end-effector / gripper frame — between the fingertips; grasp poses, tool offsets, and delta-action commands are expressed here or relative to here.
• camera mount and camera body — where the bracket holds the camera, and the camera's own body frame millimetres away.
• camera optical frames — one per sensor stream: color, depth, each infrared imager. On a D435-class device the color sensor sits roughly 15 mm from the depth origin; ignore that and every fused point is biased by 15 mm.
• object / task frame — attached to the thing being manipulated, estimated by perception, moving whenever the object does.

That is roughly fifteen frames for a one-camera cell; the WidowX AI's wrist-mounted D405 pushes it near twenty. Every arrow between frames is a rigid transform, and nearly every classic manipulation failure is one arrow being wrong: grasps that consistently miss 3 cm left (a stale camera extrinsic), a fine-tuned policy that mirrors its motions (actions logged in the wrong frame), a depth cloud where the table stands up like a wall (a missing optical-frame rotation). The folk theorem — every robotics bug is a frames bug until proven otherwise — survives because frame errors produce plausible geometry, not crashes. This lesson's job is to take away the plausibility.

Rotations: four ways to write SO(3), and what each one costs

Orientation is where the representation trouble concentrates, so start there. The rigid rotations of 3-space form the group $SO(3)$ — special orthogonal: linear maps preserving lengths and angles (orthogonal) and handedness (special: determinant $+1$, not the $-1$ of a mirror).

Rotation matrices are the workhorse: composition is matmul, applying to a point is a matvec, no singularities anywhere, and orthogonality hands you the inverse for free: $R^{-1} = R^\top$. Two costs: 9 floats for 3 degrees of freedom, and after thousands of composed multiplications, floating-point drift walks the product off the manifold — columns stop being unit-length and orthogonal — so you must project back (SVD or Gram–Schmidt). And never forget that composition does not commute: rotate a book 90° about its spine then 90° about the vertical axis, then swap the order, and you get visibly different results. Order is physical, not notational.

Axis-angle exploits Euler's rotation theorem: every rotation is a single turn by angle $\theta$ about some unit axis $\hat{n}$, packed as the rotation vector $\theta\hat{n}$ — minimal, 3 floats, no constraint to maintain. It is the natural language for small rotations — orientation errors, angular velocities, the residuals a controller or learned policy outputs — because near the identity it behaves almost linearly. (Formally it is the tangent space of $SO(3)$, connected to the matrix form by the exponential map; Phase 02's kinematics meets that machinery properly.) Its weaknesses: no closed-form composition — convert, multiply, convert back — and the axis becomes ambiguous as $\theta \to \pi$.

Unit quaternions are why your middleware messages have four fields. A quaternion packs the same axis-angle data as $q = (\cos\frac{\theta}{2},\; \hat{n}\sin\frac{\theta}{2})$: four floats, one constraint ($\|q\|=1$). Composition is a 16-multiply product, drift repair is a trivial renormalization (compare re-orthonormalizing a matrix), and interpolation has a clean constant-speed answer (slerp) — which is why ROS and every robot logging format standardize on quaternions for storage and transport. But the representation has one deep quirk you must internalize:

The half-angle inside the formula is the culprit: the quaternion completes only half a cycle while the physical rotation completes a full one, so the 3-sphere of unit quaternions maps 2-to-1 onto $SO(3)$. Consequences are everywhere. Orientation distance must be sign-aware — use the angle $2\arccos|q_1 \cdot q_2|$. Slerp must flip one endpoint's sign when $q_1 \cdot q_2 < 0$ or it takes the long way around. Componentwise averaging across a log silently mixes hemispheres. And training a network to regress quaternions from a dataset containing both signs gives it two contradictory labels for one orientation — one reason modern pose estimators prefer rotation-matrix or 6D outputs. One shallower but expensive trap: SciPy and ROS store quaternions scalar-last $(x, y, z, w)$ while Eigen's constructor takes the scalar first. A swapped $w$ is a large, valid-looking rotation.

SE(3): the 4×4 matrix that carries rotation and translation together

A general rigid motion rotates and then translates: $p' = Rp + t$. That affine form composes awkwardly — chain two and the translations tangle with the rotations. The homogeneous-coordinates trick appends a 1 to every point and packs the motion into one 4×4 matrix, making composition pure matmul. The set of all such matrices is $SE(3)$, the special Euclidean group — the configuration space of every rigid body in your cell.

Composition is now matrix multiplication, applied right to left like function composition: $T_2 T_1 p$ applies $T_1$ first, and non-commutativity carries over with translations joining the tangle. The inverse is the one formula worth deriving rather than memorizing, because the naive guess is wrong in exactly the plausible-garbage way this lesson is about. Set up the unknown inverse in block form and demand that the product be the identity:

Read off the two block equations. The rotation block gives $RA = I$, so $A = R^{-1} = R^\top$ — orthogonality hands you the inverse rotation as a transpose, no linear solve required. The translation block gives $Rb + t = 0$, so $b = -R^\top t$. Note what $b$ is not: it is not $-t$; the old translation must be rotated into the new frame before negation.

One more distinction the 4×4 form encodes elegantly: points versus vectors. A point (a grasp location) has a definite place, so translation applies — homogeneous coordinate 1. A vector (a ray direction, an angular velocity, a surface normal) has magnitude and direction but no place, so translation must not apply — homogeneous coordinate 0:

The transform tree: names that make inversion bugs impossible

Now the bookkeeping convention everything else in this course rests on. Write every transform as T_parent_child, defined to map coordinates from the child frame into the parent frame: if $p_B$ is a point expressed in frame $B$, then $p_A = T_{AB}\, p_B$. Equivalently, $T_{AB}$ is the pose of frame B as seen from A — its translation column is B's origin in A-coordinates, its rotation columns are B's axes in A-coordinates. This is exactly the semantics of ROS tf2, where frames are strings, every frame has exactly one parent, and the cell forms a tree: no cycles, so exactly one chain of transforms connects any two frames.

The tree has two kinds of edges. Static transforms — camera mount to body, body to each optical frame — are fixed by machining, CAD, or calibration; publish them once. Dynamic transforms — the arm's link-to-link chain — are recomputed from the joint encoders at the controller's update rate (500 Hz on the WidowX AI); composing base-to-gripper through six joint-dependent links is forward kinematics, which Phase 02 treats properly. The consequence for your logging: a transform is only valid together with a timestamp, because on a moving arm $T_{\text{base,ee}}$ is a function of time. tf2 buffers transforms with timestamps and interpolates on lookup for exactly this reason — the next lesson is about what those timestamps actually mean.

Trust nothing: verify every chain numerically

Frames discipline is a habit, and habits need enforcement. Three checks cost microseconds and catch most transform bugs before they touch hardware: (1) rotation validity — $R^\top R = I$ to tight tolerance and $\det R = +1$, catching transposition-instead-of-inversion, drift, and accidental mirrors; (2) round trip — a chain composed with its inverse must reproduce the input to near machine precision; (3) physical plausibility — transform a point whose answer you know from a tape measure. Here is the full pattern on a realistic three-frame chain: arm base, a camera mounted 15 cm behind the base and 40 cm up, pitched 40° down over the workspace, and an object seen 55 cm away along the optical axis.

import numpy as np

def rot_y(theta_rad):
    c, s = np.cos(theta_rad), np.sin(theta_rad)
    return np.array([[c, 0.0, s],
                     [0.0, 1.0, 0.0],
                     [-s, 0.0, c]])

def make_T(R, t):
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def inv_T(T):
    # The closed form we derived: [R^T, -R^T t], NOT [R^T, -t].
    R, t = T[:3, :3], T[:3, 3]
    return make_T(R.T, -R.T @ t)

def check_rotation(R, name, atol=1e-9):
    ortho = np.allclose(R.T @ R, np.eye(3), atol=atol)
    proper = abs(np.linalg.det(R) - 1.0) < atol
    if not (ortho and proper):
        raise ValueError(name + " is not a valid rotation")

# Frame 1: camera mount posed in the arm base frame.
# 150 mm behind the base origin, 400 mm up, pitched 40 deg down.
T_base_mount = make_T(rot_y(np.deg2rad(40.0)),
                      np.array([-0.150, 0.0, 0.400]))

# Frame 2: optical frame posed in the mount (body) frame.
# Body convention (x fwd, y left, z up) -> optical convention
# (z fwd, x right, y down), plus the color sensor's 15 mm offset.
R_mount_opt = np.array([[0.0,  0.0, 1.0],
                        [-1.0, 0.0, 0.0],
                        [0.0, -1.0, 0.0]])
T_mount_opt = make_T(R_mount_opt, np.array([0.0, -0.015, 0.0]))

# Frame 3: perception reports the object in the optical frame:
# 20 mm right of image center, 50 mm below, 550 mm along the axis.
p_opt = np.array([0.020, 0.050, 0.550, 1.0])  # homogeneous point

# Compose. Read it out loud: base-from-mount, mount-from-optical.
T_base_opt = T_base_mount @ T_mount_opt

# Check 1: still a proper rotation after composition.
check_rotation(T_base_opt[:3, :3], "R_base_opt")

# Check 2: round trip to machine precision, and the closed-form
# inverse must agree with the generic solver.
assert np.allclose(inv_T(T_base_opt) @ T_base_opt, np.eye(4), atol=1e-12)
assert np.allclose(inv_T(T_base_opt), np.linalg.inv(T_base_opt), atol=1e-12)

# Check 3: physical plausibility.
p_base = T_base_opt @ p_opt
print("object in base frame [m]:", np.round(p_base[:3], 4))
# -> [ 0.2392 -0.035   0.0082]
# 24 cm ahead of the base, 3.5 cm right, 8 mm above the table
# plane -- reachable (WidowX AI reach ~0.77 m) and ON the table.

# A direction transforms with homogeneous coordinate 0:
table_normal_opt = inv_T(T_base_opt) @ np.array([0.0, 0.0, 1.0, 0.0])
assert abs(np.linalg.norm(table_normal_opt[:3]) - 1.0) < 1e-12

The plausibility check is doing real work in that printout: the object lands at $z = 8$ mm, essentially on the table plane, where objects live. Now make the classic mistake and use inv_T(T_base_opt) @ p_opt instead: the result, about $(-0.07, -0.22, 0.03)$, is 36 cm from the truth yet also near table height and also inside the workspace. That is the treachery of inversion bugs: they do not always look absurd. So plausibility alone is not enough — set an object at a taped mark and demand the chain reproduce that ground-truth point to within a centimetre.

Also know how sensitive the chain is — that number sets Phase 02's calibration requirements. Rotation errors are amplified by lever arm: a small angular error $\delta\theta$ in the camera's orientation displaces a point at distance $r$ by approximately

This is where the lesson becomes the course's load-bearing wall. Phase 02's camera calibration is nothing but estimating T_base_cam from data — this lesson tells you what that matrix means and how to check the estimate. And in Phases 03–04, every dataset and policy defines an action space in some frame, typically delta end-effector poses in the arm base frame. Fine-tuning a π₀-class model from the OpenPI stack (opens in a new tab) on your own WidowX AI means writing, by hand, the transform chain from the policy's output convention to your joint commands, and from your logged observations back into the dataset's convention. Get one link wrong and you train on a lie, with losses that converge beautifully on data that does not describe your robot.

Where this goes next: the previous lesson, Inside the arm: actuators, encoders, and control modes, gave you joint angles as trustworthy numbers; this lesson turned those numbers into geometry — a tree of frames where every point, pose, and action has a well-defined home. But a transform is only correct at the instant it was measured: on a moving arm, $T_{\text{base,ee}}$ is sampled by encoders, shipped over the network, and consumed milliseconds later. The next lesson, Time is a sensor: clocks, timestamps, and latency, gives every transform its fourth coordinate — together they define the episode logger you will build in Lab 0.