X0.2.1

Lesson 0.2.1 — Homogeneous Transformations and SE(3): Why Robotics Uses 4×4 Matrices

Week 3 · Prerequisites: 0.1.1, 0.1.2, 0.1.3, 0.1.4.

Prior results used. This lesson uses 0.1.1’s tensor model (batch dimensions, dtype, device, @ matmul), 0.1.2’s autograd over a chain of matmuls, and 0.1.3’s nn.Linear weight-shape convention; each is restated at the point of use.

Hands forward. This lesson hands forward one capability: the learner can represent a rigid body pose as a 4×4 matrix, compose a chain of transforms in the correct order, invert a transform analytically, distinguish a point from a direction, and reason about the degrees of freedom a transform actually encodes — the capability that every subsequent robotics lesson (1.1.1 forward kinematics, 1.1.2 inverse kinematics, 2.1.1 pose estimation, 2.4.2 SLAM) assumes.

Running case. The same 6-DOF MuJoCo arm from Lessons 0.1.1–0.1.4: six revolute joints with alternating z and y axes, equal link lengths a = 0.2, and the end-effector at the end of the sixth link. Its pose is the product T_1 · T_2 · T_3 · T_4 · T_5 · T_6 with T_i = Rot_{axis_i}(q_i) · Trans_x(a), and every example in this lesson is an instance of one of those factors or of the product.

§L.0 — Lesson map, objectives, and misconceptions

§L.0.1 — Lesson map

§L.0.2 — Objectives

By the end of this lesson you will be able to do eight things:

1. State what a rigid body pose is ((R, t) with R ∈ SO(3), t ∈ ℝ³), and derive the composition rule (R₁R₂, R₁t₂ + t₁) from the point-map definition.
2. Explain why the pair (R, t) is not closed under any linear composition rule, and why the 4×4 matrix makes composition linear.
3. State the three properties of a rotation matrix (RᵀR = I, det R = +1, orthogonality) and compute the 3 degrees of freedom from the 9 entries and the constraints.
4. Write the 4×4 homogeneous transform for a given (R, t) and compute its action on a point ([p; 1]) and a vector ([v; 0]) by hand, showing the difference.
5. Derive the analytic inverse T⁻¹ = (Rᵀ, -Rᵀt), and explain why the naive (Rᵀ, -t) is wrong.
6. Apply the composition property to the running case’s 6-DOF forward kinematics, computing the end-effector position as a rotated sum of link vectors, by hand for a 2-link sub-chain and in code for the full chain.
7. Diagnose three silent transform bugs — wrong inverse, wrong composition order, wrong point/vector convention — from the symptom each produces.
8. Produce a written “transform audit” of a forward-kinematics pipeline: every frame, every transform, the composition order, and the frame each translation is expressed in.

§L.0.3 — Misconception inventory

Each misconception is displaced in the section named, and each is tagged in the concept check that follows it.

Everything is self-contained. Where a prerequisite lesson is used, the result is restated at the point of use.

§L.0.5 — Retrieval warm-up

This is the fifth lesson of the curriculum. The warm-up draws from Lessons 0.1.3, 0.1.2, and 0.1.1 — 0.1.4 is only one step back and is excluded.

Warm-up 1 (from 0.1.1, batch shapes). A batched tensor of B rotation matrices has shape (B, 3, 3), and a batched tensor of B points has shape (B, 3). Write the expression that applies each rotation to its point and state the result shape. Answer: reshape the points to (B, 3, 1), compute R @ p ((B, 3, 3) @ (B, 3, 1) → (B, 3, 1)), then .squeeze(-1) → (B, 3). How this is used: §L.4’s batched FK applies each joint’s 4×4 transform to a batch of points with the same pattern, extended to the 4-vector homogeneous point.

Warm-up 2 (from 0.1.2, autograd over matmul chains). A chain T₁ @ T₂ @ T₃ @ p builds a graph of @ operations. Does .backward() on a scalar depending on the result flow gradients through all three matrices? Answer: Yes — each @ is a matmul node; the backward pass computes the gradient with respect to each matrix by the chain rule, and each matrix’s .grad is populated if it is a leaf with requires_grad=True. How this is used: §L.4’s FK chain’s gradients with respect to the joint transforms — the Jacobian of Lesson 1.1.2 — flow through this chain.

Warm-up 3 (from 0.1.3, nn.Linear’s weight shape). nn.Linear(3, 3) has a weight of shape (3, 3) and a forward pass x @ W.T + b. Could a learner use this as a learnable rotation matrix? Answer: No — nn.Linear’s weight is an arbitrary 3×3 matrix, and its output is not constrained to satisfy RᵀR = I or det R = +1; a gradient step on W does not preserve either constraint, so the output is a general linear map (stretching, possibly reflecting), not a rotation. The learnable-rotation question is resolved in 0.2.2 by learning 3 unconstrained parameters and converting to R. How this is used: §L.2’s 3-DOF discussion is the motivation for 0.2.2, and §L.0.2’s eighth objective is to recognize that a 3×3 matrix that does not satisfy the constraints is not a rotation.

Warm-up 4 (from 0.1.1, dtype and precision). A 4×4 transform is stored as float32 or float64. For a chain of 1,000 matrix multiplications, which dtype preserves RᵀR = I more accurately? Answer: float64 — float32’s accumulated error over 1,000 matmuls is roughly 1000 · 10⁻⁷ = 10⁻⁴, which is visible in RᵀR - I, while float64’s error is roughly 10⁻¹³, negligible. How this is used: §L.4’s lab tests RᵀR error after a chain, and the choice of dtype for the running case’s transforms is stated explicitly.

Part A — Rigid Body Motion and Why 4×4

§L.1 — Objects: rotations, translations, and the composition problem

A rigid body motion (a pose, an element of SE(3)) is a map on ℝ³ that preserves distances and orientations, and every such map can be written uniquely as p ↦ Rp + t with R ∈ SO(3) (the rotation) and t ∈ ℝ³ (the translation).

The pose of a frame B relative to a frame A, written T_A^B or T_{A←B}, maps B-expressed quantities to A-expressed quantities: p^A = R_A^B p^B + t_A^B. The subscript reads “A from B” — the map’s target first, its source second — and the convention is universal in robotics.

The composition problem: given T_A^B = (R_A^B, t_A^B) and T_B^C = (R_B^C, t_B^C), what is T_A^C?

Derivation. For p^C, p^A = R_A^B (R_B^C p^C + t_B^C) + t_A^B = (R_A^B R_B^C) p^C + (R_A^B t_B^C + t_A^B). So T_A^C = (R_A^B R_B^C, \; R_A^B t_B^C + t_A^B): the rotation composes by matrix multiplication, and the translation composes by rotating the second translation into the first frame, then adding.

§L.1.1 — Result: (R, t) is not closed under any linear composition rule, and the 4×4 matrix makes composition linear

Claim. The composed pose (R₁R₂, R₁t₂ + t₁) is not a linear function of the pairs considered as 6-vectors, because the translation depends on the product R₁t₂. The 4×4 matrix T = [[R, t], [0, 1]] is the smallest linear representation in which composition is the ordinary matrix product.

Proof sketch. The translation’s composition involves products of R₁’s entries with t₂’s entries (nine quadratic terms) and is therefore not linear in the pair of 9-vectors. The 4×4 matrix promotes the affine map to a linear map on homogeneous coordinates: [p; 1] ↦ [Rp + t; 1] = T [p; 1], and the composition of two such maps is the product of the matrices, which is linear. The 4×4 form is the smallest square matrix in which the translation is a linear operation on the homogeneous coordinate; a 3×3 matrix cannot represent a translation (it fixes the origin), and a 3×4 or 4×3 is not square and does not compose by standard multiplication.

Corollary. The 4×4 form is not a notational convenience; it is the representation in which composition is matrix multiplication, which allows a chain of n poses to be a single @ expression and the chain’s gradient to be computed by autograd’s matmul backward (Lesson 0.1.2, warm-up 2).

§L.1.2 — Worked-example ladder

WE-L.1.a — Fully worked: composition on a 2D example, by hand

Input. Two 2D poses: T₁ = (Rz(90°), [1, 0]) and T₂ = (I, [0, 1]).

Trace (by the composition rule).

1. R₁R₂ = Rz(90°) · I = Rz(90°) = [[0, -1], [1, 0]].
2. R₁ t₂ = Rz(90°) · [0, 1] = [-1, 0] — the second translation, rotated into the first frame.
3. R₁ t₂ + t₁ = [-1, 0] + [1, 0] = [0, 0] — the composed translation is zero, not [1, 1].
4. T₁ · T₂ = (Rz(90°), [0, 0]).

Trace (by 3×3 matrix product, for cross-check).

5. M₁ = [[0, -1, 1], [1, 0, 0], [0, 0, 1]], M₂ = [[1, 0, 0], [0, 1, 1], [0, 0, 1]].
6. M₁ M₂[0, :] = [0·1 + (-1)·0 + 1·0, 0·0 + (-1)·1 + 1·0, 0·0 + (-1)·1 + 1·1] = [0, -1, 0].
7. M₁ M₂[1, :] = [1·1 + 0·0 + 0·0, 1·0 + 0·1 + 0·0, 1·0 + 0·1 + 0·1] = [1, 0, 0].
8. M₁ M₂ = [[0, -1, 0], [1, 0, 0], [0, 0, 1]] = (Rz(90°), [0, 0]). ✓

The third term of step 6 is M₂[2, 1] = 0, not 1: M₂’s rows are [1,0,0], [0,1,1], [0,0,1], so its second column is [0, 1, 0]. Reading the column index off the row index — taking M₂[1, 2] = 1 where the sum wants M₂[2, 1] — is the commonest hand-arithmetic slip in this product, and it is why the rotation block [[0, 0], [1, 0]] would come out singular and the conclusion “this is Rz(90°)” would be false while looking verified.

Output and interpretation. The composed translation is [0, 0], not the naive t₁ + t₂ = [1, 1], because t₂ = [0, 1] is rotated by R₁ = 90° to [-1, 0] before being added. The naive rule is wrong by exactly the rotation’s action on the second translation. The 3×3 matrix product gives the same result — the 4×4 (or 3×3 in 2D) matrix is the composition rule.

Self-explanation 1. Before reading on: in step 2, why is R₁ t₂ = [-1, 0] and not [0, 1]? What is the geometric meaning?

Answer. t₂ = [0, 1] points along y; rotating it by Rz(90°) maps y → -x, giving [-1, 0]. The rotation expresses t₂ in T₁’s frame, which is rotated 90° from T₂’s parent. The geometric meaning: T₂’s translation is in its own parent frame, which is T₁’s child frame; to add it to T₁’s translation (in T₁’s parent frame), it must be rotated into the parent frame first.

Self-explanation 2. Before reading on: the composed translation is [0, 0], but the naive sum is [1, 1]. What path does a point at T₂’s origin take, and what path does the naive sum represent?

Answer. The correct path: [0,0] → T₂ → [0,1] → T₁ → R₁·[0,1] + [1,0] = [-1,0] + [1,0] = [0,0]. The naive path: [0,0] → T₂ (naively additive) → [0,1] → T₁ (naively additive) → [1, 1]. The naive path treats the translations as if they were in the same frame, which they are not.

WE-L.1.b — Completion problem

Input. T₁ = (Rz(90°), [0, 1]), T₂ = (I, [1, 0]).

Trace (final 25% as prompts).

1. R₁R₂ = Rz(90°).
2. R₁ t₂ = Rz(90°) · [1, 0] = [0, 1].
3. R₁ t₂ + t₁ = [0, 1] + [0, 1] = [0, 2].
4. T₁ · T₂ = (Rz(90°), [0, 2]). ⟨your turn: verify by multiplying the 3×3 matrices, showing the top-right column.⟩
5. T₂ · T₁ = (R₂ R₁, R₂ t₁ + t₂) = (Rz(90°), I·[0,1] + [1,0]) = (Rz(90°), [1, 1]). ⟨your turn: compute M₂ · M₁ and confirm the top-right column is [1, 1].⟩
6. Compare: ⟨your turn: are the two composed translations the same? What does the difference mean?⟩

Answers.

4. M₁ = [[0, -1, 0], [1, 0, 1], [0, 0, 1]], M₂ = [[1, 0, 1], [0, 1, 0], [0, 0, 1]]. M₁ M₂[0, 2] = 0·1 + (-1)·0 + 0·1 = 0; M₁ M₂[1, 2] = 1·1 + 0·0 + 1·1 = 2. So the top-right is [0, 2]. ✓
5. M₂ M₁[0, 2] = 1·0 + 0·1 + 1·1 = 1; M₂ M₁[1, 2] = 0·0 + 1·1 + 0·1 = 1. So the top-right is [1, 1]. ✓
6. They are different — [0, 2] vs. [1, 1] — with the same rotation. The difference is the order: T₁·T₂ rotates t₂ into T₁’s frame (so [1,0] → [0,1]) before adding t₁ = [0,1]; T₂·T₁ adds t₁ in T₂’s frame (R₂ = I, no rotation) then t₂ = [1,0], giving [1,1].

The lesson. T₁·T₂ ≠ T₂·T₁ — the composition is not commutative. The difference is the rotation’s action on the second operand’s translation, and in a chain the order is the frame structure.

WE-L.1.c — Faded: the running case’s two joints

Setup. The running case’s T_1 = Rot_z(q_1)·Trans_x(a) and T_2 = Rot_y(q_2)·Trans_x(a), with a = 0.2, q_1 = 0.3, q_2 = 0.5. Each factor’s translation block is R_i · [a, 0, 0], because Trans_x(a) is applied after the joint rotation.

Question. Compute T_1 · T_2’s translation, showing each step.

You must produce the intermediate steps. The setup and the answer are given.

Answer.

• R_1 ≈ [[0.955, -0.296, 0], [0.296, 0.955, 0], [0, 0, 1]], t_1 = R_1 [a, 0, 0] ≈ [0.1911, 0.0591, 0].
• R_2 ≈ [[0.878, 0, 0.479], [0, 1, 0], [-0.479, 0, 0.878]], t_2 = R_2 [a, 0, 0] ≈ [0.1755, 0, -0.0959].
• R_1 t_2 ≈ [0.1676, 0.0519, -0.0959].
• R_1 t_2 + t_1 ≈ [0.3587, 0.1110, -0.0959].

The naive sum t_1 + t_2 ≈ [0.3666, 0.0591, -0.0959] differs by [-0.0078, 0.0519, 0.0] — the R_1 rotation of t_2.

The lesson. Each joint’s link-offset translation is rotated by the upstream joints’ rotations, so the end-effector’s position is a rotated sum of link vectors, not a sum of link lengths.

WE-L.1.d — Contrastive near-miss: (R₁R₂, t₁ + t₂)

Correct solution. (R₁R₂, R₁t₂ + t₁) — rotate t₂ into T₁’s frame, then add.

Near-miss. (R₁R₂, t₁ + t₂) — add the translations without rotating.

Why the near-miss produces a plausible but incorrect number. The result is a legal (R, t) — a valid rotation and a valid translation — so the framework, the type system, and the downstream code all accept it. The end-effector position is a different position (differing by (R₁ - I)t₂), the Jacobian computed from it is a different Jacobian, and the policy trained on it is a policy for a different robot — one whose second link is positioned by t₁ + t₂ rather than by R₁t₂ + t₁. The failure is silent in the code and visible only against a ground-truth FK.

The discrepancy, stated. R₁t₂ + t₁ and t₁ + t₂ differ by the R₁ factor on t₂, which is the second translation’s rotation into the first frame. Omitting it treats the two translations as if in the same frame, which they are not. The bug is silent for a pure translation and catastrophic for a chain with rotations — which is the running case.

Self-explanation 3. Before reading on: if T₁ has R₁ = I, does the near-miss coincide with the correct composition?

Answer. Yes — if R₁ = I, R₁t₂ + t₁ = t₂ + t₁ = t₁ + t₂, so the two coincide. The bug matters only when R₁ ≠ I; a learner who tests only translations can miss it.

§L.1.3 — Comparative table: representations of a rigid body motion

Design principle (P5.1). When composition will happen, use the 4×4 matrix. The pair (R, t) is the right representation for reading a transform’s components; the 4×4 is the right representation for composing a chain (T₁ @ T₂ @ … @ Tₙ) and for applying it (T @ [p; 1]). In PyTorch, the 4×4 is the default and the pair is a view of its blocks.

§L.1.4 — Concept check

Concept check (displaces M1). A learner says: “The 4×4 matrix is notation; I can compose pairs with (R₁R₂, t₁ + t₂).” Name the missing term, and the two things the 4×4 provides that the pair does not.

Answer. Missing term: R₁t₂ — the second translation must be rotated into the first frame before adding. The 4×4 provides (i) the linear composition rule — the product, with no rule to remember — and (ii) the linear action on points, T @ [p; 1], with the translation inside the matrix rather than a separate addition.

§L.1.5 — Consolidation checkpoint 1

(a) You should now be able to: (i) state the pose convention T_A^B and derive the composition rule; (ii) explain why (R₁R₂, R₁t₂ + t₁) is nonlinear in the pairs and why the 4×4 is linear; (iii) state the three reasons the 4×4 is the right representation.

(b) Transfer probe. A learner composes T₁·T₂ with R₁ = Rz(90°) and R₂ = Rx(90°), and compares to T₂·T₁. Does the order matter? Answer sketch: Yes — the products Rz(90°)·Rx(90°) and Rx(90°)·Rz(90°) are different rotations, and their angles are not the sum of the individual angles (which is what a naive “90 + 90 = 180” would suggest). The order determines which rotation is applied in whose frame.

(c) Remediation rule. If you cannot do (b), return to §L.1.2 and redo WE-L.1.b, then re-attempt (b) by writing the two products explicitly.

Part B — Rotation Matrices and SO(3)

§L.2 — Objects: SO(3), its properties, and its degrees of freedom

The special orthogonal group SO(3) is the set of 3×3 matrices R with RᵀR = I (orthogonality) and det R = +1 (special). The group operations are closure (product of rotations is a rotation), identity (I), inverse (Rᵀ), and associativity.

The properties relevant to robotics: det = +1 distinguishes a rotation from a reflection (det = -1); orthogonality means R preserves lengths (‖Rp‖ = ‖p‖) and angles (⟨Rp, Rq⟩ = ⟨p, q⟩); and Rᵀ = R⁻¹ makes the inverse free — no arithmetic.

The degrees of freedom are 3: 9 entries minus the 6 independent orthogonality constraints (the symmetric 3×3 equation RᵀR = I gives 3 diagonal = 1 and 3 off-diagonal = 0), with det = +1 selecting the rotation component (the reflections form the other det = -1 component).

The elementary rotations:

• Rz(θ) = [[c, -s, 0], [s, c, 0], [0, 0, 1]]
• Ry(θ) = [[c, 0, s], [0, 1, 0], [-s, 0, c]]
• Rx(θ) = [[1, 0, 0], [0, c, -s], [0, s, c]]

with c = cos θ, s = sin θ. Each satisfies RᵀR = I and det = +1 by the Pythagorean identity c² + s² = 1.

§L.2.1 — Result: SO(3) is a 3-dimensional group, and a general 3×3 matrix is not a rotation

Claim. Every R ∈ SO(3) is a rotation matrix, and SO(3) is a 3-dimensional smooth manifold. A general 3×3 matrix not satisfying the constraints is a general linear map, not a rotation — it may stretch and reflect.

Proof sketch. RᵀR = I is 6 independent equations in 9 entries, so the solution set is a 3-dimensional manifold; det = +1 selects one connected component (the rotations) of the det = ±1 set (the reflections form the other). A general matrix not satisfying the constraints is not in SO(3), and its action can change lengths and orientations.

Corollary. A network that outputs a 3×3 matrix is not outputting a rotation. Its output is a general linear map, and using it as a rotation introduces stretching and reflection into the pose. The fix (0.2.2) is to learn 3 unconstrained parameters (a quaternion or exponential coordinate) and convert them to R, so the learned quantity is unconstrained and the rotation is always a proper SO(3) element.

Corollary. The SO(3)-invariant distance is θ = arccos((tr(R₁ᵀR₂) - 1)/2), the rotation angle of the relative rotation R₁ᵀR₂. The Frobenius norm is also invariant — ‖R₁ - R₂‖_F = √(6 - 2tr(R₁ᵀR₂)) = √8·sin(θ/2), which depends on θ alone and is unchanged by conjugating both matrices by the same Q — so invariance does not distinguish the two. What distinguishes them is the shape: ‖·‖_F is a bounded, saturating function of θ (√8 ≈ 2.828 at θ = π), and its gradient d‖·‖_F/dθ = (√8/2)·cos(θ/2) falls to zero at the half-turn, while θ grows linearly on [0, π] with a constant gradient 1. A pose loss trained on the entry norm therefore has almost no signal exactly where the error is largest, which is why the geodesic angle — not the entry norm — is the loss to use.

§L.2.2 — Worked-example ladder

WE-L.2.a — Fully worked: verifying a rotation’s properties, by hand

Input. R = Rz(π/2) = [[0, -1, 0], [1, 0, 0], [0, 0, 1]].

Trace (verify RᵀR = I).

1. Rᵀ = [[0, 1, 0], [-1, 0, 0], [0, 0, 1]].
2. RᵀR[0, 0] = 0·0 + 1·1 + 0·0 = 1.
3. RᵀR[0, 1] = 0·(-1) + 1·0 + 0·0 = 0.
4. RᵀR[1, 1] = (-1)·(-1) + 0·0 + 0·0 = 1.
5. All off-diagonal entries are 0, so RᵀR = I. ✓

Trace (verify det R = +1).

6. det R = 0·(0·1 - 0·0) - (-1)·(1·1 - 0·0) + 0 = 0 + 1 + 0 = 1. ✓

Trace (verify length preservation).

7. p = [1, 0, 0], Rp = [0, 1, 0], ‖p‖ = ‖Rp‖ = 1. ✓

Output and interpretation. All three properties hold. The learner who computed det = -1 has made a sign error in the cofactor expansion; the learner who found RᵀR ≠ I has used the transpose incorrectly.

Self-explanation 4. Before reading on: in step 7, p = [1, 0, 0] → Rp = [0, 1, 0]. What is the geometric meaning?

Answer. The x-axis maps to the y-axis, which is a 90° rotation about the z-axis — exactly Rz(π/2). The z-axis is fixed (a rotation about z leaves z unchanged).

WE-L.2.b — Completion problem

Input. M = [[1, 1, 0], [0, 1, 0], [0, 0, 1]] — a shear in the xy-plane.

Trace (final 25% as prompts).

1. MᵀM[0, 0] = 1·1 + 0·0 + 0·0 = 1; MᵀM[0, 1] = 1·1 + 0·1 + 0·0 = 1. ⟨your turn: is MᵀM = I?⟩
2. det M = 1·(1·1 - 0·0) - 1·(0·1 - 0·0) + 0 = 1. ⟨your turn: does det M = +1 suffice for M ∈ SO(3)?⟩
3. M · [0, 1, 0] = [1, 1, 0]; ‖[0, 1, 0]‖ = 1 but ‖[1, 1, 0]‖ = √2. ⟨your turn: does M preserve length?⟩

Answers.

1. No — MᵀM[0, 1] = 1 ≠ 0; M is not orthogonal, hence not in SO(3).
2. No — det = +1 is necessary but not sufficient; orthogonality is also required.
3. No — M stretches the y-axis by √2; it does not preserve length.

The lesson. det = +1 alone does not make a rotation; the orthogonality is the additional structure. The check is MᵀM = I (equivalently M Mᵀ = I or Mᵀ = M⁻¹), and a learned 3×3 matrix that fails it is not a rotation.

WE-L.2.c — Faded: the running case’s first two rotations

Setup. R_1 = Rz(0.3), R_2 = Ry(0.5).

Question. Compute R_1 R_2 and R_2 R_1, and state whether they are equal.

You must produce the intermediate steps. The setup and the answer are given.

Answer.

• R_1 R_2 ≈ [[0.838, -0.296, 0.457], [0.260, 0.955, 0.142], [-0.479, 0, 0.878]].
• R_2 R_1 ≈ [[0.838, -0.260, 0.479], [0.296, 0.955, 0], [-0.457, 0.142, 0.878]].
• They differ (e.g., [0, 1] is -0.296 vs. -0.260; [0, 2] is 0.457 vs. 0.479), and the difference is the non-commutativity of rotation composition.

The lesson. The composition of two rotations about different axes is not commutative; the order encodes which rotation is applied in whose frame.

WE-L.2.d — Contrastive near-miss: a learned 3×3 matrix used as a rotation

Correct solution. Learn a quaternion q (4 numbers, ‖q‖ = 1 constraint, 3 DOF), convert q to R, use R.

Near-miss. Learn a 3×3 matrix R_learned (9 free numbers), use it as a rotation.

Why the near-miss produces a plausible but incorrect number. The training loss decreases — the network can fit the training keypoints with a general linear map, which has 9 parameters vs. the rotation’s 3. The fitted R_learned is close to the ground-truth R in the entries but is not orthogonal and may have det < 0 (a reflection); it stretches the object and mirrors it. The deployed “pose” is not a rigid body’s, and the rigid gripper cannot match it. The failure is silent in the loss and visible only in the singular values of R_learned (which are not all 1) or in the physical grasp.

The discrepancy, stated. The 9-parameter parameterization does not encode the “output is a rotation” prior; the 3-parameter does. The learned 3×3 is a general linear map (with 9 DOF, of which 6 are not rotations), and a gradient step almost surely leaves SO(3) because the constraints are measure-zero in the 9-dimensional space.

Self-explanation 5. Before reading on: why does the loss decrease for the 9-parameter version, and why is the 3-parameter version’s loss higher (but the output correct)?

Answer. The 9-parameter network has more capacity, so it can fit the training keypoints’ noise and any non-rigid deformation in the data, giving a lower loss; the 3-parameter network is restricted to rigid motions, so it cannot fit the noise, giving a higher training loss but a correct rotation. The 3-parameter version is the right choice when the data is a rigid-body image of a known object; the extra capacity of the 9-parameter version is used to fit noise, which does not generalize.

§L.2.3 — Comparative table: rotation representations

Design principle (P5.2). Learn 3 numbers, compute with 3×3, display as needed. The learnable representation is the unconstrained 3-parameter one (quaternion or exponential coordinate); the computational representation is the 3×3 matrix; the user-facing representation is whatever the human needs. The pipeline converts between them at the boundaries. nn.Linear(3, 3)’s 9-parameter output is the wrong learnable side; 0.2.2 replaces it.

§L.2.4 — Concept check

Concept check (displaces M4). A learner says: “A 3×3 rotation matrix has 9 degrees of freedom.” Give the correct count, and the two consequences for learning.

Answer. The count is 3 (9 entries minus 6 independent orthogonality constraints; det = +1 selects the component). Consequences: (i) a 9-parameter learned matrix is over-parameterized (6 redundant directions), and (ii) it is almost surely not in SO(3) (the constraints are measure-zero), so the output stretches and possibly reflects. The fix is to learn 3 parameters and convert to R.

§L.2.5 — Consolidation checkpoint 2

(a) You should now be able to: (i) state the two defining properties of SO(3) and verify them for a matrix; (ii) compute the 3 DOF from the 9 entries and constraints; (iii) state the three elementary rotations and their geometric meanings; (iv) state the 3-parameter learning fix and the SO(3)-invariant distance.

(b) Transfer probe. A learner’s network outputs a 3×3 M and the loss is ‖M - R_gt‖_F. Does the minimum correspond to M = R_gt? Answer sketch: Yes — over all 3×3 matrices the unique minimizer of ‖M - R_gt‖_F² is M = R_gt itself, which is a rotation, so the unconstrained minimum is already feasible and a stretch strictly increases the loss (a 5% stretch costs 0.005 in the squared norm). The loss is nonetheless the wrong instrument, for two reasons. It saturates: restricted to SO(3) its maximum is 8 at θ = π, where its gradient in θ is zero, so the largest errors are the ones it can barely see; and it is √8·sin(θ/2), not θ, so the ranking it induces is a monotone but compressive reparametrization of the geodesic one — errors of 1.0 and 2.0 rad are ranked 1.36 and 2.38 apart instead of a factor of two. The SO(3)-invariant loss is θ² where θ = arccos((tr(R_gtᵀ M) - 1)/2), which vanishes at M = R_gt, is independent of the basis, and has the gradient that matches the error.

(c) Remediation rule. If you cannot do (b), return to §L.2.1 and re-read the “SO(3)-invariant distance” corollary, then re-attempt (b) by asking whether the loss changes if both M and R_gt are multiplied by the same rotation.

Part C — Homogeneous Transforms and SE(3)

§L.3 — Objects: SE(3), composition, inverse, and action

The special Euclidean group SE(3) is the set of 4×4 matrices T = [[R, t], [0, 1]] with R ∈ SO(3) and t ∈ ℝ³. Its group operations:

• Closure: T₁T₂ = [[R₁R₂, R₁t₂ + t₁], [0, 1]] ∈ SE(3).
• Identity: I₄.
• Inverse: T⁻¹ = [[Rᵀ, -Rᵀt], [0, 1]] (derived below).
• Associativity: inherited from matrix multiplication.

The inverse’s derivation. Solve T T⁻¹ = I₄ with T⁻¹ = [[R', t'], [0, 1]]: the top-left block gives R R' = I → R' = Rᵀ; the top-right block gives R t' + t = 0 → t' = -Rᵀt. So T⁻¹ = [[Rᵀ, -Rᵀt], [0, 1]].

The action on a point p: T [p; 1] = [Rp + t; 1]. The action on a direction v (velocity, force, direction): T [v; 0] = [Rv; 0]. The last homogeneous component is 1 for a point and 0 for a direction.

§L.3.1 — Result: the inverse is (Rᵀ, -Rᵀt), and the naive (Rᵀ, -t) is wrong unless Rt = t

Claim. T⁻¹ = [[Rᵀ, -Rᵀt], [0, 1]], and [[Rᵀ, -t], [0, 1]] is the inverse only when Rt = t.

Proof sketch. As above, T⁻¹ must satisfy R t' + t = 0, so t' = -Rᵀt. The naive t' = -t gives R(-t) + t = t - Rt, which is 0 only if Rt = t, i.e., t is on the rotation axis (or R = I or t = 0).

Corollary. The inverse’s translation is in the child frame, and -Rᵀt is the parent-frame translation -t converted to the child frame. The naive -t is in the parent frame, which is the wrong frame.

§L.3.2 — Worked-example ladder

WE-L.3.a — Fully worked: the wrong inverse and the right inverse, side by side

Input. T = [[0, -1, 0, 1], [1, 0, 0, 2], [0, 0, 1, 3], [0, 0, 0, 1]] — Rz(90°) with t = [1, 2, 3].

Trace (correct inverse).

1. R = Rz(90°), Rᵀ = [[0, 1, 0], [-1, 0, 0], [0, 0, 1]], t = [1, 2, 3].
2. Rᵀ t = [0·1 + 1·2, (-1)·1 + 0·2, 0·1 + 0·2 + 1·3] = [2, -1, 3].
3. -Rᵀ t = [-2, 1, -3].
4. T⁻¹ = [[0, 1, 0, -2], [-1, 0, 0, 1], [0, 0, 1, -3], [0, 0, 0, 1]].
5. Verify: R Rᵀ = I, R(-Rᵀ t) + t = -R Rᵀ t + t = -t + t = [0, 0, 0]. ✓

Trace (naive inverse, for contrast).

6. T⁻¹_naive = [[0, 1, 0, -1], [-1, 0, 0, -2], [0, 0, 1, -3], [0, 0, 0, 1]].
7. Verify: R(-t) + t = t - R t = [1, 2, 3] - [0·1 - 1·2, 1·1 + 0·2, 3] = [1, 2, 3] - [-2, 1, 3] = [3, 1, 0]. Not [0, 0, 0] — the naive is not the inverse.

Output and interpretation. The correct inverse’s translation is -Rᵀt = [-2, 1, -3], the naive’s is -t = [-1, -2, -3], and their difference is (-t) - (-Rᵀt) = Rᵀt - t = [1, -3, 0]. Step 7’s residual is a different quantity, the translation of the product T⁻¹_naive·T, which is t - R t = [3, 1, 0] — the same vector rotated by R, since R(Rᵀt - t) = t - Rt. Both are non-zero for the same reason: the naive inverse fails by exactly the amount the rotation moves the translation, and since a robot arm’s translation is off the joint axis in almost every configuration, the naive is wrong in almost every configuration.

Self-explanation 6. Before reading on: what is the geometric meaning of t - R t, and when does it vanish?

Answer. t - R t is the component of t perpendicular to the rotation axis — the part of t that the rotation moves. It vanishes when t is along the rotation axis (the axial component is unchanged by the rotation), which is exactly the case where the naive inverse coincides with the correct one.

WE-L.3.b — Completion problem

Input. T = [[1, 0, 0, 4], [0, 0, -1, 5], [0, 1, 0, 6], [0, 0, 0, 1]] — Rx(90°) with t = [4, 5, 6].

Trace (final 25% as prompts).

1. R = [[1, 0, 0], [0, 0, -1], [0, 1, 0]], Rᵀ = [[1, 0, 0], [0, 0, 1], [0, -1, 0]].
2. Rᵀ t = [4, 6, -5]. ⟨your turn: show the arithmetic⟩
3. -Rᵀ t = [-4, -6, 5]. ⟨your turn: write the inverse’s 4×4 matrix⟩
4. Verify: R(-Rᵀ t) + t. ⟨your turn: compute, showing = [0, 0, 0]⟩

Answers.

2. Rᵀ t = [1·4 + 0·5 + 0·6, 0·4 + 0·5 + 1·6, 0·4 + (-1)·5 + 0·6] = [4, 6, -5].
3. T⁻¹ = [[1, 0, 0, -4], [0, 0, 1, -6], [0, -1, 0, 5], [0, 0, 0, 1]].
4. R · [-4, -6, 5] = [-4, -5, -6]; + [4, 5, 6] = [0, 0, 0]. ✓

The lesson. The inverse’s translation is -Rᵀ t, and the algebraic check R(-Rᵀ t) + t = 0 is the correctness test.

WE-L.3.c — Faded: the identity-rotation case

Setup. T = [[I, t], [0, 1]] (identity rotation, translation t = [1, 2, 3]), and p_ee = [0.1, 0.2, 0.3].

Question. Compute p_base = T [p_ee; 1] and p_recovered = T⁻¹ [p_base; 1], and verify recovery. Then state what happens with the naive inverse.

You must produce the intermediate steps. The setup and the answer are given.

Answer. p_base = [0.1 + 1, 0.2 + 2, 0.3 + 3] = [1.1, 2.2, 3.3]. T⁻¹ for R = I is [[I, -t], [0, 1]] (since Rᵀ = I and -Rᵀt = -t); p_recovered = [1.1 - 1, 2.2 - 2, 3.3 - 3] = [0.1, 0.2, 0.3]. ✓ The naive inverse is also [[I, -t], [0, 1]] (since Rᵀ = I and -t = -Rᵀt), so it is correct for R = I — and a test on the identity rotation does not distinguish the two inverses.

The lesson. The naive inverse coincides with the correct one when R = I; a test that passes on the trivial case is not evidence of correctness on the non-trivial case.

WE-L.3.d — Contrastive near-miss: the wrong point/vector convention

Correct solution. A velocity v (a direction) is transformed by T [v; 0] = [Rv; 0], giving Rv — rotated but not translated.

Near-miss. The learner applies T [v; 1] = [Rv + t; 1], giving Rv + t — rotated and translated. The velocity is now Rv + t, biased by t.

Why the near-miss produces a plausible but incorrect number. The result is a legal 3-vector with the correct orientation (Rv) and an extra constant t. In a control loop, the velocity is the true velocity plus a constant offset in the frame’s direction; the robot drifts with a systematic bias that does not average out over trials (it is deterministic for a given frame). The learner who attributes the drift to “sensor noise” has missed the convention bug.

The discrepancy, stated. T [v; 0] and T [v; 1] differ by t, the translation the point convention adds and the direction convention omits. The two are the same formula with a different last component, and the last component is the type of the object.

Self-explanation 7. Before reading on: if R = I and the transform is a pure translation, is the velocity’s biased value the same in every frame?

Answer. No — T [v; 1] = v + t is frame-dependent (different frames have different t), while the true velocity is v in every frame (a direction is a free vector). The biased value’s frame-dependence is observable as a discrepancy between the velocity’s value in two frames that differ by a pure translation, which the true velocity does not have.

§L.3.3 — Comparative table: the four SE(3) operations

Design principle (P5.3). Distinguish points from directions at the type level, not just the convention. Have two functions — transform_point(T, p) and transform_vector(T, v) — with the convention inside the function. The function name is the type, and the type is the check.

§L.3.4 — Concept check

Concept check (displaces M2, M5). A learner’s transform(T, x) uses [x; 1] for both points and velocities. Which use is correct, and what is the symptom for the other?

Answer. The point use is correct; the velocity use is wrong — it receives the spurious translation t, and the symptom is a systematic, frame-dependent bias in the velocity (a constant offset that does not average out over trials). The fix is a separate transform_vector with [v; 0], and the two function names state the type.

§L.3.5 — Consolidation checkpoint 3

(a) You should now be able to: (i) state the four SE(3) operations and the four formulas; (ii) derive the inverse and state why the naive fails; (iii) state the point-vs-vector convention and its symptom.

(b) Transfer probe. A learner’s transform(T, f) transforms a force f, and the result is larger than expected by a constant. What is the bug? Answer sketch: the force is a direction, so it should use [f; 0]; the learner used [f; 1], adding t to the force. The extra t is the “larger by a constant” — the fix is transform_vector(T, f).

(c) Remediation rule. If you cannot do (b), return to §L.3.2 and redo WE-L.3.d, then re-attempt (b) by asking whether a force is a point or a direction.

Part D — The Running Case: 6-DOF Forward Kinematics

§L.4 — Objects: the chain, the joint transform, and the end-effector pose

The running case’s forward kinematics is T_0^6(q) = T_1(q_1)·T_2(q_2)·T_3(q_3)·T_4(q_4)·T_5(q_5)·T_6(q_6), where T_i = Rot_{axis_i}(q_i)·Trans_x(a) is the joint’s transform from joint i-1’s frame to joint i’s frame (axis_i alternates z, y, z, y, z, y). Because Trans_x(a) is applied after the rotation, the factor’s translation block is R_i·[a, 0, 0], not [a, 0, 0] — the sixth link is the sixth factor’s own Trans_x(a), and there is no seventh factor. The end-effector’s position is the top-right 3×1 block of T_0^6.

§L.4.1 — Result: the end-effector position is a rotated sum of link vectors, not a sum of link vectors

Claim. p_ee(q) = R_1 a + R_1 R_2 a + R_1 R_2 R_3 a + R_1 R_2 R_3 R_4 a + R_1 R_2 R_3 R_4 R_5 a + R_1 R_2 R_3 R_4 R_5 R_6 a, where each a = [a, 0, 0] is the link vector and R_i = R_{axis_i}(q_i).

Proof sketch. Expand the product using the composition rule. The first Trans_x(a) is rotated by no preceding factor; the second by R_1; the third by R_1R_2; and so on. The sum is the displayed formula, with each term the link vector rotated by the upstream joints.

Corollary. The naive sum 6a is the arm’s maximum reach (achieved only at q = 0), not its actual reach. The actual p_ee is inside that boundary for non-zero joint angles.

Corollary. The end-effector’s orientation is R_1R_2R_3R_4R_5R_6, a pure matrix product. The 4×4 representation captures both the position’s rotated sum and the orientation’s pure product in one object.

§L.4.2 — Worked-example ladder

WE-L.4.a — Fully worked: the 2-link sub-chain, by hand

Input. T_1 = Rot_z(q_1)·Trans_x(a), T_2 = Rot_y(q_2)·Trans_x(a), with a = 0.2, q_1 = π/2, q_2 = 0. The end-effector is the origin of frame 2, so p_ee is the translation block of T_1 · T_2.

Trace.

1. R_1 = Rz(π/2) = [[0, -1, 0], [1, 0, 0], [0, 0, 1]]; t_1 = R_1 [a, 0, 0] = [0, a, 0] = [0, 0.2, 0] — the first link points along y, because the joint rotation precedes the link offset.
2. R_2 = Ry(0) = I; t_2 = R_2 [a, 0, 0] = [0.2, 0, 0].
3. R_1 t_2 = Rz(π/2) [0.2, 0, 0] = [0, 0.2, 0] — the second link, expressed in the base frame, is also along y.
4. R_1 t_2 + t_1 = [0, 0.2, 0] + [0, 0.2, 0] = [0, 0.4, 0].
5. p_ee = [0, 0.4, 0].

Trace (naive sum, for contrast).

6. The naive sum a + a = [0.4, 0, 0] treats both links as unrotated along x.

Output and interpretation. p_ee = [0, 0.4, 0] vs. naive [0.4, 0, 0], differing by [-0.4, 0.4, 0] — the q_1 = 90° rotation applied to both links. The naive sum is for a different arm (two straight links of length 0.2 along x); the correct one is for the 2-link arm bent 90° at the base.

Self-explanation 8. Before reading on: why does q_2 not affect p_ee in this configuration?

Answer. Because q_2 = 0 — the second joint’s rotation is the identity, so R_2 = I and the second link’s direction is unchanged by q_2. For q_2 ≠ 0, R_2 would rotate the second link, and p_ee would depend on q_2 (see WE-L.4.b).

WE-L.4.b — Completion problem

Input. The same 2-link chain with q_1 = π/2, q_2 = π/2, a = 0.2.

Trace (final 25% as prompts).

1. R_2 = Ry(π/2) = [[0, 0, 1], [0, 1, 0], [-1, 0, 0]]; R_2 [a, 0, 0] = [0, 0, -a].
2. t_2 = R_2 [a, 0, 0] = [0, 0, -a], and R_1 = Rz(π/2), t_1 = [0, a, 0]. ⟨your turn: verify⟩
3. R_1 t_2 = Rz(π/2) [0, 0, -a] = [0, 0, -a]. ⟨your turn: compute and state p_ee⟩

Answers.

2. R_2 [a, 0, 0] = [0, 0, -a] — the second link now points along -z. ✓
3. R_1 t_2 + t_1 = [0, 0, -a] + [0, a, 0] = [0, a, -a] = [0, 0.2, -0.2].

The lesson. With q_2 = π/2, p_ee = [0, 0.2, -0.2] — different from WE-L.4.a’s [0, 0.4, 0], because q_2 rotates the second link into the -z direction.

WE-L.4.c — Faded: the running case with one parameter changed

Setup. The full 6-DOF chain at q = [0.3, 0.5, -0.2, 0.1, 0.4, -0.3], a = 0.2.

Question. Write the expression for p_ee as a rotated sum, and explain why the sixth joint’s rotation affects the position even though T_6’s origin is the sixth joint.

You must produce the intermediate steps. The setup and the answer are given.

Answer. p_ee = R_1a + R_1R_2a + R_1R_2R_3a + R_1R_2R_3R_4a + R_1R_2R_3R_4R_5a + R_1R_2R_3R_4R_5R_6a, where a = [a, 0, 0]. The sixth joint’s rotation R_6 appears in the last term: Trans_x(a) is applied after Rot_{axis_6}(q_6), so the sixth link’s vector is R_6a expressed in frame 5, and then rotated by all five upstream joints. The sixth joint’s origin is the sum of the first five terms; the end-effector is that origin plus R_1...R_6a. So q_6 moves the end-effector through the sixth link’s orientation.

The lesson. Because the link offset is applied after the joint rotation, every link contributes a term rotated by its own and all upstream joints. That is why the last joint moves the end-effector, and why the naive 6a is the reach only at q = 0.

WE-L.4.d — Contrastive near-miss: the wrong composition order in FK

Correct solution. T_0^6 = T_1 · T_2 · ... · T_6, with T_i in the chain’s order.

Near-miss. T_0^6 = T_6 · T_5 · ... · T_1, with the chain reversed.

Why the near-miss produces a plausible but incorrect number. The reversed product is a legal SE(3) element (a valid rigid motion), so the end-effector pose is a legal pose and the code runs. But the reversed product is the FK of a different arm — one whose joints are ordered in reverse — and its end-effector position differs from the correct one by an amount that grows with the joint angles. The failure is silent and visible only against the correct FK (a test the learner would have to write).

The discrepancy, stated. The product’s order encodes the frame chain: T_1·T_2 applies T_2 in T_1’s child frame, and T_2·T_1 applies T_1 in T_2’s child frame — different arms. The correct order is the chain order (from base to end-effector), and reversing it gives the “inside-out” arm.

Self-explanation 9. Before reading on: in the near-miss, does the reversed product have the same rotation as the correct one? Why or why not?

Answer. No — the rotation is R_6R_5...R_1 (reversed) vs. R_1...R_6 (correct), and these are different rotations (the composition is non-commutative). The reversed arm’s end-effector has a different orientation, which is the same bug as the different position, one level up.

§L.4.3 — Comparative table: FK implementation pitfalls

Design principle (P5.4). Verify FK against a known geometric configuration. For q = 0, the arm is fully extended and p_ee = [6a, 0, 0] = [1.2, 0, 0]. For q = [π/2, 0, ..., 0], the first joint has swung the whole arm 90° about z, and the end-effector is at [0, 1.2, 0]. These two tests catch the composition order, the composition rule, and a misplaced link offset in one line each.

§L.4.4 — Concept check

Concept check (displaces M3 in FK). A learner’s FK returns p_ee = [1.2, 0, 0] for q = 0 (correct) but p_ee = [1.2, 0, 0] for q = [π/2, 0, ..., 0] too (wrong). What is the bug?

Answer. The second test should give [0, 1.2, 0] (the arm rotated 90° about z), and the learner’s output [1.2, 0, 0] is the naive sum 6a — the FK is not applying the joint rotations to the downstream links, likely because the composition uses t₁ + t₂ + ... instead of R₁t₂ + t₁, or because the transforms are applied in the wrong order. The q = 0 test cannot distinguish the two, because at q = 0 all rotations are the identity and both the naive and correct rules give 6a; the q = π/2 test distinguishes them.

§L.4.5 — Consolidation checkpoint 4

(a) You should now be able to: (i) write the 6-DOF FK as a product of joint transforms and state the end-effector’s rotated-sum position; (ii) compute p_ee by hand for a 2-link sub-chain; (iii) diagnose a wrong-order FK, a wrong-rule FK, and a misplaced link offset, and state the test that distinguishes each.

(b) Transfer probe. A learner’s 6-DOF FK gives the correct p_ee for all q but the wrong orientation for non-zero q. What is the most likely bug? Answer sketch: the position is correct (the translation’s composition is right) but the orientation is wrong — the R block of the product is being computed as a sum or as R₁ + R₂ + ... instead of R₁R₂...R₆, or the rotations are being applied in the wrong order. The orientation’s correct composition is a pure matrix product, and the test is to compare R_0^6 against R_1R_2...R_6 for a specific q.

(c) Remediation rule. If you cannot do (b), return to §L.4.2 and redo WE-L.4.d, then re-attempt (b) by writing the rotation block’s expected product.

Part E — Failure Taxonomy, Lab, Cheat Sheet, Exercises, Exit Test

§L.5 — Failure taxonomy

The table covers the lab’s three break-it steps (wrong inverse, wrong order, wrong convention) plus two not exercised there (learned 3×3, all-6a).

§L.6 — The lab

§L.6.1 — Task

Implement a Transform module in PyTorch (functions for make_T, apply_point, apply_vector, invert, compose) and use it to verify the composition rule, the inverse, the point/vector convention, and the 6-DOF FK; then break the pipeline three ways.

§L.6.2 — Conditions

Python 3.10+, PyTorch 2.2+, CPU only. All runs use torch.manual_seed(42). dtype = float64 for the transforms (to avoid float32 accumulation in the RᵀR check). Runtime: under 1 minute.

§L.6.3 — Controls held fixed

The seed (42), the dtype (float64), the link length (a = 0.2), and the joint configuration (q = [0.3, 0.5, -0.2, 0.1, 0.4, -0.3]) are fixed across all steps.

§L.6.4 — Metrics table

§L.6.5 — Pre-registered hypotheses

• H1. identity_error is < 1e-12 (the correct inverse is exact up to float64 rounding), and naive_identity_error > 1e-3 (the naive inverse is wrong by a visible amount). The prediction is the pair of bounds.
• H2. composition_order_diff > 0.1 for a pair where the first transform carries a 90° rotation and the second a non-zero translation. The prediction is > 0.1.
• H3. point_vs_vector_diff equals ‖t‖ (the translation’s norm) for the same input and the same transform. The prediction is ‖t‖ exactly.
• H4. fk_q0 = 1.2 (the maximum reach 6a) and fk_q_nonzero is strictly inside the reach ‖p_ee‖ < 1.2. The prediction is the pair (fk_q0 = 1.2, ‖fk_q_nonzero‖ < 1.2).

§L.6.6 — The break-it sequence

1. The naive inverse (silent). Replace invert with invert_wrong = (Rᵀ, -t); run the identity check. Predicted symptom: no exception; ‖T·T⁻¹_wrong - I‖ is > 1e-3 for any R ≠ I and t off the rotation axis. Actual: as predicted — silent.
2. The wrong composition order (silent). Reverse the product to T_6·...·T_1. Predicted symptom: no exception; the end-effector position differs from the correct one. Actual: as predicted — silent.
3. The wrong point/vector convention (silent). Transform a velocity with [v; 1] instead of [v; 0]. Predicted symptom: no exception; the velocity is biased by t. Actual: as predicted — silent.

All three are silent, exceeding the minimum of two.

§L.6.7 — Acceptance criteria

§L.6.8 — What good looks like

A strong submission has five properties:

1. The analytic formulas are implemented as functions, not as inline matrices. make_T(R, t), apply_point(T, p), apply_vector(T, v), invert(T), compose(T1, T2) are all separate functions with the convention inside them; the inline matrices are in the test code, not the library code.
2. The naive inverse’s failure is shown numerically, not just asserted. The submission prints identity_error for both the correct and the naive inverse, with the two numbers, so the correctness and the failure are both visible.
3. The point/vector difference is exactly ‖t‖. The submission prints the difference and the translation’s norm, and the two are equal (up to float64 rounding); the equality is the demonstration that the translation was the only difference.
4. The FK’s q = 0 test equals 6a, and the q ≠ 0 test is inside the reach. Both numbers are printed, and the difference between them is the rotations’ effect. The learner who prints only one has not distinguished the naive sum from the correct FK.
5. The break-its are predicted before running. The predictions are specific numeric bounds (H1–H4), and the actuals are compared against them; the comparison is the diagnostic skill, and the predictions are its evidence.

§L.6.9 — Runnable lab code

# lab_0_2_1.py — Homogeneous transforms, SE(3), and the 6-DOF FK
# Requires: Python 3.10+, PyTorch 2.2+

import torch

torch.manual_seed(42)
dtype = torch.float64
device = torch.device("cpu")

# --- Step 1: the transform constructors --------------------------------
def make_T(R, t):
    """R: (3,3), t: (3,) -> (4,4) homogeneous transform."""
    T = torch.eye(4, dtype=dtype, device=device)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def rot_z(q):
    c, s = torch.cos(q), torch.sin(q)
    return torch.tensor([[c, -s, 0], [s, c, 0], [0, 0, 1]], dtype=dtype, device=device)

def rot_y(q):
    c, s = torch.cos(q), torch.sin(q)
    return torch.tensor([[c, 0, s], [0, 1, 0], [-s, 0, c]], dtype=dtype, device=device)

# --- Step 2: point vs vector action -----------------------------------
def apply_point(T, p):
    p_h = torch.cat([p, torch.ones(1, dtype=dtype, device=device)])
    return (T @ p_h)[:3]

def apply_vector(T, v):
    v_h = torch.cat([v, torch.zeros(1, dtype=dtype, device=device)])
    return (T @ v_h)[:3]

# --- Step 3: the inverse and the naive inverse ------------------------
def invert(T):
    R, t = T[:3, :3], T[:3, 3]
    return make_T(R.T, -R.T @ t)

def invert_wrong(T):
    R, t = T[:3, :3], T[:3, 3]
    return make_T(R.T, -t)

# --- Step 4: the test transform ---------------------------------------
R = rot_z(torch.tensor(torch.pi / 2, dtype=dtype, device=device))
t = torch.tensor([1.0, 2.0, 3.0], dtype=dtype, device=device)
T = make_T(R, t)

I4 = torch.eye(4, dtype=dtype, device=device)
err_correct = (T @ invert(T) - I4).abs().max().item()
err_naive   = (T @ invert_wrong(T) - I4).abs().max().item()
print(f"identity_error (correct): {err_correct:.2e}")   # < 1e-12
print(f"identity_error (naive):   {err_naive:.2e}")     # > 1e-3

# --- Step 5: the composition order ------------------------------------
T1 = make_T(rot_z(torch.tensor(torch.pi / 2, dtype=dtype, device=device)),
            torch.tensor([1.0, 0.0, 0.0], dtype=dtype, device=device))
T2 = make_T(torch.eye(3, dtype=dtype, device=device),
            torch.tensor([0.0, 1.0, 0.0], dtype=dtype, device=device))
p = torch.tensor([1.0, 0.0, 0.0], dtype=dtype, device=device)
p_12 = apply_point(T1 @ T2, p)
p_21 = apply_point(T2 @ T1, p)
print(f"p (T1·T2): {p_12}")
print(f"p (T2·T1): {p_21}")
print(f"composition_order_diff: {(p_12 - p_21).norm().item():.4f}")

# --- Step 6: point vs vector -----------------------------------------
v = torch.tensor([1.0, 0.0, 0.0], dtype=dtype, device=device)
p_as_point  = apply_point(T, v)
p_as_vector = apply_vector(T, v)
print(f"v as point:  {p_as_point}")
print(f"v as vector: {p_as_vector}")
print(f"point_vs_vector_diff: {(p_as_point - p_as_vector).norm().item():.6f}")
print(f"||t||: {t.norm().item():.6f}")

# --- Step 7: the 6-DOF FK --------------------------------------------
# T_i = Rot_axis(q_i) . Trans_x(a). Trans_x is applied AFTER the rotation,
# so the factor's translation block is R_i @ [a, 0, 0], not [a, 0, 0].
LINK = torch.tensor([0.2, 0.0, 0.0], dtype=dtype, device=device)

def joint_T(R):
    """One joint: rotation R followed by the link offset a along the rotated x."""
    return make_T(R, R @ LINK)

def fk_6(q):
    axes = [rot_z, rot_y, rot_z, rot_y, rot_z, rot_y]
    T = torch.eye(4, dtype=dtype, device=device)
    for i in range(6):
        T = T @ joint_T(axes[i](q[i]))
    return T

q0 = torch.zeros(6, dtype=dtype, device=device)
T_q0 = fk_6(q0)
print(f"fk at q=0: {T_q0[:3, 3].tolist()}")              # should be [1.2, 0, 0]

q = torch.tensor([0.3, 0.5, -0.2, 0.1, 0.4, -0.3], dtype=dtype, device=device)
T_q = fk_6(q)
print(f"fk at q={q.tolist()}: {T_q[:3, 3].tolist()}")
print(f"||fk at q||: {T_q[:3, 3].norm().item():.6f}")     # should be < 1.2
print(f"reach (6a): {1.2:.6f}")

# --- Step 8: break-it #1 — the naive inverse (silent) ----------------
print(f"[break-it #1] T·T^-1_naive has translation: {(T @ invert_wrong(T))[:3, 3].tolist()}")
# should be nonzero (e.g., [3, 1, 0] for this T)

# --- Step 9: break-it #2 — the wrong composition order (silent) ------
def fk_6_reversed(q):
    axes = [rot_z, rot_y, rot_z, rot_y, rot_z, rot_y]
    T = torch.eye(4, dtype=dtype, device=device)
    for i in range(5, -1, -1):
        T = T @ joint_T(axes[i](q[i]))
    return T

print(f"[break-it #2] fk (reversed) at q: {fk_6_reversed(q)[:3, 3].tolist()}")
print(f"           vs. correct: {T_q[:3, 3].tolist()}")

# --- Step 10: break-it #3 — the wrong point/vector convention (silent)
v_biased = apply_point(T, v)
print(f"[break-it #3] velocity as point: {v_biased.tolist()} (biased by t = {t.tolist()})")
print(f"           as vector: {apply_vector(T, v).tolist()}")

# --- Step 11: summary ------------------------------------------------
print("\n--- lab 0.2.1 summary ---")
print(f"H1: identity_error correct={err_correct:.2e}, naive={err_naive:.2e}")
print(f"H2: composition_order_diff={(p_12 - p_21).norm().item():.4f} (predicted > 0.1)")
print(f"H3: point_vs_vector_diff={(p_as_point - p_as_vector).norm().item():.6f}, ||t||={t.norm().item():.6f}")
print(f"H4: fk(q=0)={T_q0[:3, 3].tolist()}, ||fk(q)||={T_q[:3, 3].norm().item():.4f}")
print("silent break-its: #1 (naive inverse), #2 (reversed order), #3 (wrong convention)")

§L.7 — The cheat sheet

§L.8 — Exercises with solutions

E 0.2.1.1 — The composition by hand (numerical). T₁ = (Rz(90°), [2, 0, 0]), T₂ = (I, [0, 3, 0]). Compute T₁·T₂’s translation and T₂·T₁’s translation, and state which one’s end-effector is at [2, 3, 0].

S 0.2.1.1. T₁·T₂: R₁t₂ + t₁ = Rz(90°)·[0,3,0] + [2,0,0] = [-3,0,0] + [2,0,0] = [-1, 0, 0]. T₂·T₁: R₂t₁ + t₂ = [2,0,0] + [0,3,0] = [2, 3, 0]. So T₂·T₁’s translation is [2, 3, 0] — the second composition (translate first, then rotate) is the one at [2, 3, 0]. The lesson: the order determines the frame each translation is in, and the “translate then rotate” vs. “rotate then translate” give different end-effector positions; the naive sum [2, 3, 0] matches the second order, not the first, and the learner who expects the naive sum for the first order is wrong.

E 0.2.1.2 — The inverse by hand (numerical). T = [[0, -1, 0, 3], [1, 0, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]]. Compute T⁻¹, verify T·T⁻¹ = I₄, and compute the naive inverse’s error.

S 0.2.1.2. R = Rz(90°), t = [3, 4, 5]. Rᵀt = [4, -3, 5], -Rᵀt = [-4, 3, -5]. T⁻¹ = [[0, 1, 0, -4], [-1, 0, 0, 3], [0, 0, 1, -5], [0, 0, 0, 1]]. Verify: R·Rᵀ = I, R(-Rᵀt) + t = -t + t = [0, 0, 0]. Naive: t' = -t = [-3, -4, -5], and T·T⁻¹_naive’s translation is R(-t) + t = t - Rt = [3,4,5] - [-4,3,5] = [7, 1, 0]. The lesson: the inverse’s translation is -Rᵀt, and the naive -t is off by Rᵀt - t = [1, -7, 0] in the child frame — which is the product residual t - Rt = [7, 1, 0] rotated back, Rᵀ(t - Rt). Quote the frame with the vector: the two spellings differ by a rotation, so an answer that names only one is ambiguous.

E 0.2.1.3 — Point vs. vector (diagnostic). A learner applies T to a velocity v = [1, 0, 0] and gets [4, 5, 6] where T has R = I and t = [3, 5, 6]. What convention did they use, and what should the answer be?

S 0.2.1.3. The output [4, 5, 6] = v + t = [1, 0, 0] + [3, 5, 6] — the learner used the point convention [v; 1]. The correct velocity convention is [v; 0]: T [v; 0] = Rv = [1, 0, 0]. The output should be [1, 0, 0], not [4, 5, 6]; the difference is the spurious translation t, and the symptom is a systematic bias in the velocity. The lesson: the last homogeneous component is the type, and a velocity’s type is 0; using 1 adds the translation, which is a direction’s bug.

E 0.2.1.4 — The 3 DOF count (numerical). A 3×3 matrix has 9 entries. How many independent constraints does RᵀR = I impose, and what is the resulting DOF count? Why is det R = +1 not an additional independent constraint for the DOF count?

S 0.2.1.4. RᵀR is a symmetric 3×3 matrix, so it has 6 independent entries (3 diagonal, 3 off-diagonal), and RᵀR = I gives 6 independent equations. So 9 - 6 = 3. det R = +1 is not independent of orthogonality — orthogonality implies det R = ±1, so the det condition selects the +1 component (the rotations) from the ±1 set (rotations + reflections), but does not further reduce the dimension. So the count is 3, and the det is a sign choice on the 6-constraint manifold. The lesson: the DOF count is 9 - 6 = 3, and det = +1 selects which of the two components (rotations vs. reflections) the count is on.

E 0.2.1.5 — The FK test (diagnostic). A learner’s 6-DOF FK gives p_ee = [1.2, 0, 0] for both q = 0 and q = [π/2, 0, ..., 0]. What is the bug?

S 0.2.1.5. The correct q = π/2 output is [0, 1.2, 0] (the arm rotated 90° about z), and the learner’s [1.2, 0, 0] is the naive sum 6a, unchanged by q. The FK is not applying the joint rotations to the downstream links — likely the composition uses t₁ + t₂ + ... instead of R₁t₂ + t₁, or the transforms are in the wrong order, or q is not being used. The q = 0 test cannot distinguish the naive from the correct rule (both give 6a at q = 0), and the q = π/2 test is the discriminator. The lesson: the q = 0 test is necessary and not sufficient; the non-zero-q test is the one that exercises the rotations.

E 0.2.1.6 — The learnable-3×3 diagnosis (transfer probe). A learner’s network outputs a 3×3 matrix M as a rotation, trained with the loss ‖Mp - R_gt p‖² over a set of points p. The training loss reaches zero. Is the learned M a rotation? What should the learner check?

S 0.2.1.6. The training loss reaching zero means M maps the training points to the correct locations, but it does not mean M is a rotation — M is a 9-parameter linear map, and it can fit the points with a stretching or reflection that a rotation cannot. The check is MᵀM (should be I) and det M (should be +1); if either fails, M is not a rotation. The fix is to parameterize the rotation with 3 numbers (quaternion or exponential coordinate) and convert to R, so the output is always in SO(3). The lesson: a loss that reaches zero on the training points does not certify that the map is a rotation; the constraints (MᵀM = I, det M = +1) are the certificate, and the 3-parameter parameterization is what makes them hold by construction.

E 0.2.1.7 — The composition-order diagnosis (diagnostic, transfer probe). A learner’s FK gives p_ee = [0, 0.4, 0] for the 2-link sub-chain at q = [π/2, 0] (correct) and p_ee = [0.4, 0, 0] for the same q after they “optimized” the code. What is the likely change?

S 0.2.1.7. The correct p_ee = [0, 0.4, 0] has both links along y: t_1 = R_1[a,0,0] = [0,0.2,0] and R_1 t_2 = [0,0.2,0], so the first joint’s 90° rotation has swung the whole arm. The “optimized” [0.4, 0, 0] is the naive sum — the rotations are no longer applied to the link offsets at all, which is the signature of summing the link lengths instead of composing the transforms. This is not the offset-before-rotation bug, though the two are easy to confuse: building each factor as make_T(R_i, [a,0,0]) also drops the rotation from the offset, but the upstream rotations still apply to everything downstream, so the 2-link chain lands at [0.2, 0.2, 0] and the full chain at [0.2, 1.0, 0] — a bent pose with the right q = 0 reach, not a straight one. The fix is to apply the offset after the rotation: t_i = R_i @ [a, 0, 0]. The lesson: both bugs share a q = 0 symptom, because at q = 0 every rotation is the identity and all three builds give 6a; only a q ≠ 0 test separates them, and only the position of the bent pose separates offset-before from naive-sum.

E 0.2.1.8 — The FK implementation (constructive). Write pseudocode for a forward_kinematics(q, joint_axes, link_lengths, ee_offset) function that composes the joint transforms in the correct order and returns the end-effector position. Then state the test that would catch a wrong composition order.

S 0.2.1.8.

function forward_kinematics(q, joint_axes, link_lengths):
    T = identity(4)
    for i in 0 .. n-1:
        R_i = elementary_rotation(joint_axes[i], q[i])
        # the offset is applied AFTER the rotation, so it is R_i @ (L_i * x_axis)
        T_i = make_T(R_i, R_i @ (link_lengths[i] * x_axis))
        T = T @ T_i
    return T[:3, 3]  # end-effector position

function test_fk():
    q0 = zeros(n)
    assert close(forward_kinematics(q0, ...), [sum(link_lengths), 0, 0])
    q1 = [pi/2, 0, ..., 0]
    # the first joint swings the whole arm 90 deg about z, so the end-effector
    # leaves the x-axis for the y-axis at the same reach
    p = forward_kinematics(q1, ...)
    assert close(p, [0, sum(link_lengths), 0])

The q = 0 test verifies the reach; the q = π/2 test verifies that the rotations are being applied (the wrong order would put the end-effector at a different point). The lesson: the FK’s correctness is tested by two configurations — q = 0 (the reach) and q = π/2 (the rotation’s effect) — and the second is the one that catches the composition-order bug, which the q = 0 test cannot.

§L.9 — Exit test

You are ready for Lesson 0.2.2 when you can, without notes:

1. State what a rigid body motion is, and derive the composition rule (R₁R₂, R₁t₂ + t₁) from the point-map definition.
2. Explain why the pair (R, t) is not closed under any linear composition rule, and why the 4×4 matrix makes composition linear.
3. State the two defining properties of SO(3), verify them for a given matrix, and compute the 3 DOF from the constraints.
4. State the three elementary rotations and their geometric meanings, and verify Rz(π/2)’s action on the x-axis.
5. State the 4 SE(3) operations (composition, inverse, point action, vector action) and their formulas.
6. Derive the inverse (Rᵀ, -Rᵀt) and explain why the naive (Rᵀ, -t) is wrong.
7. Compute T·T⁻¹ for a given transform, and verify the product is I for the correct inverse and is not I for the naive inverse.
8. State the point-vs-vector convention, and give the symptom of using the wrong one for a velocity.
9. Write the 6-DOF FK as a product of joint transforms, and state the end-effector’s rotated-sum position.
10. Compute p_ee for a 2-link sub-chain at q = [π/2, 0] and q = [π/2, π/2], and state the difference.
11. Diagnose a wrong-order FK, a wrong-rule FK, and a misplaced link offset, and state the test that distinguishes each.
12. State the 3-parameter learning fix for rotations, and the SO(3)-invariant distance.

If any of items 1–12 is not answerable without notes, return to the section that introduced it: item 1 to §L.1, item 2 to §L.1.1, item 3 to §L.2.1, item 4 to §L.2.2, item 5 to §L.3, item 6 to §L.3.1, item 7 to §L.3.2, item 8 to §L.3.4, item 9 to §L.4.1, item 10 to §L.4.2, item 11 to §L.5, and item 12 to §L.2.3.

§L.10 — Provenance and evidence tiers

What this lesson reproduces. Every hypothesis in §L.6.5 is executed by the lab in §L.6.9. Tier A, status R.

What this lesson does not reproduce. Nothing in it is a research claim. The homogeneous transform is a definition — a matrix whose block structure is the composition rule — and the lesson’s claims are consequences of that definition, checked by multiplication. There is no literature to tier, and the lesson does not cite any.

The four results are all of one kind, and that is the lesson’s point. Each is an identity that holds exactly in exact arithmetic, so each has a tolerance rather than an estimate: 1e-17 and 1e-16 are floating-point noise, not measurement error. A learner who reports identity_error = 1e-8 and calls it “close enough” has not understood that this quantity has no physical scale — it should be at machine precision or the algebra is wrong. That is why the lab’s thresholds are 1e-10 and not 1e-3.

No external sources are cited, and none are needed. Every claim is a definition or an algebraic identity the reader can verify by hand or in one line of NumPy.