Geometric Transformations

Chapter 1: Convex Polygons & Polyhedra

Before we can ask "does the robot collide with an obstacle?", we need a mathematical description of the robot's shape and the obstacle's shape. LaValle's approach uses semi-algebraic models built from convex primitives — and the foundation is the half-plane (2D) or half-space (3D).

Half-planes and Convex Sets

A half-plane is the set of all points (x, y) satisfying a single linear inequality: a·x + b·y ≤ c. Geometrically, this is one side of a line. A convex polygon is the intersection of finitely many half-planes. A square is the intersection of four half-planes. A regular hexagon is six. Any convex polygon with n sides is defined by exactly n half-plane inequalities.

This representation is powerful because checking whether a point is inside a convex polygon is fast — just evaluate n inequalities. And intersecting two convex polygons is algorithmically clean. The price: most real shapes are not convex. A car body, a robot hand, a coffee mug — all are non-convex.

Non-convex Bodies: Unions of Convex Parts

The standard trick: decompose a non-convex body into a finite union of convex primitives. A C-shaped robot might be represented as the union of two rectangles. The robot arm's link is a rectangle; its wrist is a small disk. When checking collision, you check each primitive separately — a point is in the non-convex body if it's in any one of its convex parts.

Convex Polyhedra in 3D

The 3D analog: a half-space is the set satisfying a·x + b·y + c·z ≤ d. A convex polyhedron is a finite intersection of half-spaces. A cube is 6 half-spaces. A tetrahedron is 4. The faces of the polyhedron correspond one-to-one with the half-space inequalities. Modern collision detection libraries (FCL, Bullet) represent robot links as convex polyhedra or triangulated meshes.

Chapter 2: Semi-algebraic Models

Half-planes give us linear boundaries — straight lines and flat faces. But what about a circle? A cylinder? A robot link with rounded ends? For these we need algebraic primitives, not just linear ones.

Algebraic Sets and Semi-algebraic Sets

An algebraic set is the zero set of a polynomial: { x : f(x) = 0 }. A circle is the algebraic set of x² + y² − r² = 0. An ellipse is ax² + by² − 1 = 0. These are the boundaries of more complex shapes.

A semi-algebraic set replaces the equality with an inequality: { x : f(x) ≤ 0 }. The disk x² + y² ≤ r² is a semi-algebraic set — the filled circle. A half-plane ax + by ≤ c is a special case (degree-1 polynomial). LaValle uses semi-algebraic sets as the general framework for describing robot bodies and obstacles, because they handle both linear boundaries (flat walls) and curved boundaries (cylinders, spheres) in a unified way.

This formula says: an obstacle O is a union (∪) of intersections (∩) of semi-algebraic primitives. The outer union handles non-convexity. The inner intersection handles each convex piece. Every robot-obstacle description in chapters 4–8 ultimately reduces to this form.

Why This Matters for Planning

When the configuration space is built in Chapter 4, the obstacles in C-space (called C-obstacles) inherit this semi-algebraic structure from the workspace obstacles and the robot body. The planner must navigate a space whose "walls" are defined by polynomial inequalities. This is why exact planners (Chapter 6) use algebraic geometry, while sampling-based planners (Chapter 5) sidestep the algebra entirely by testing random configurations.

Chapter 3: 2D Rotation & Translation

A rigid body in 2D has three degrees of freedom: two translation (x_t, y_t) and one rotation (θ). Given a point p = (a, b) expressed in the body's local frame, what are its coordinates in the world frame?

The 2D Rotation Matrix

Rotating a point (a, b) by angle θ around the origin gives:

In matrix form, with R(θ) the rotation matrix:

This is a 2×2 orthogonal matrix (R^TR = I, det R = 1). The columns of R(θ) are the body's x-axis and y-axis expressed in world coordinates. If θ = 0, R = I (identity: body and world aligned). If θ = 90°, R rotates everything 90° counterclockwise.

The set of all 2×2 rotation matrices — all matrices satisfying R^TR = I and det R = +1 — is called SO(2) (Special Orthogonal group in 2D). It's topologically a circle: each θ ∈ [0, 2π) corresponds to exactly one rotation. The + sign on the determinant excludes reflections.

Adding Translation: Homogeneous Coordinates

Rotation alone isn't enough — the body also translates by (x_t, y_t). Combining rotation and translation:

This is affine, not linear — you can't represent it as a single matrix multiplication on (x, y). The fix: homogeneous coordinates. Augment every 2D point with a 1 in the third slot: [x, y, 1]^T. Then the combined rotation + translation becomes a single 3×3 matrix multiplication:

This 3×3 matrix is the homogeneous transformation matrix for 2D rigid motion. The top-left 2×2 block is R(θ). The top-right column is [x_t, y_t]^T. The bottom row is always [0, 0, 1]. Its power: chaining two transformations (first rotate, then translate, then rotate again) is just matrix multiplication.

Chapter 4: 3D Rotations

Moving from 2D to 3D rotations is a significant step — not just because the matrices grow from 2×2 to 3×3, but because 3D rotation has a fundamentally different topology and several nasty surprises.

Rotation About a Single Axis

In 3D, we can rotate around any of the three coordinate axes. Rotation by angle α around the z-axis:

Similarly, R_x(α) rotates around the x-axis, R_y(β) around the y-axis. Each is a 3×3 orthogonal matrix with det = +1. The set of all 3×3 rotation matrices is SO(3) (Special Orthogonal group in 3D).

Yaw, Pitch, Roll — Euler Angles

Any 3D rotation can be decomposed into three successive rotations around coordinate axes. The most common convention (aerospace): yaw (rotation around z-axis, like a car turning), pitch (rotation around y-axis, like nose-up/down), roll (rotation around x-axis, like a barrel roll). The combined rotation is:

These three angles together are called Euler angles (more precisely, Tait-Bryan angles). They are widely used in robotics and aerospace because they are human-interpretable. The catch: they suffer from a phenomenon called gimbal lock.

Gimbal Lock: The Hidden Singularity

Gimbal lock occurs when one rotation aligns two rotation axes, causing the loss of one degree of freedom. For the yaw-pitch-roll convention, it happens when pitch = ±90°: at that angle, yaw and roll become equivalent — you can no longer independently control all three axes. Two of your three "knobs" do the same thing.

This is not a software bug — it's a fundamental topological fact about representing SO(3) with three angles. There is no three-parameter representation of SO(3) that avoids singularities everywhere. The solutions: use four-parameter quaternions (no singularities, compact, great for interpolation) or use the full 3×3 rotation matrix directly (9 parameters, but easy to compose).

Chapter 5: Homogeneous Transformation Matrices

The 2D homogeneous trick (3×3 matrix = rotation + translation) extends naturally to 3D: a 4×4 matrix encodes a full rigid-body transformation — 3D rotation and 3D translation in one object.

The 4×4 Homogeneous Transformation

A 3D point (x, y, z) is represented in homogeneous coordinates as [x, y, z, 1]^T. A rigid transformation (rotation R, translation t) acts as:

The upper-left 3×3 block is the rotation matrix R ∈ SO(3). The upper-right 3×1 column is the translation vector t. The bottom row [0, 0, 0, 1] is always fixed. The resulting 4×4 matrix is a member of the SE(3) group (Special Euclidean group in 3D) — the group of all rigid-body transformations.

Composing Transformations

The magic: composing two rigid transformations T₁ (first) then T₂ (second) is just matrix multiplication T = T₂ · T₁. If you rotate by θ, then translate by t, then rotate by φ, the combined transformation is R(φ) · T(t) · R(θ) — read right to left. This works because matrix multiplication is associative.

python
import numpy as np

def rot_z(theta):
    """4x4 homogeneous matrix for rotation around z-axis."""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [c, -s,  0,  0],
        [s,  c,  0,  0],
        [0,  0,  1,  0],
        [0,  0,  0,  1]
    ])

def translate(tx, ty, tz):
    """4x4 homogeneous matrix for pure translation."""
    return np.array([
        [1, 0, 0, tx],
        [0, 1, 0, ty],
        [0, 0, 1, tz],
        [0, 0, 0,  1]
    ])

# Compose: first rotate 45°, then translate (2, 1, 0)
T = translate(2, 1, 0) @ rot_z(np.pi/4)

# Transform a point at (1, 0, 0) in the body frame
p_body = np.array([1, 0, 0, 1])  # homogeneous
p_world = T @ p_body
# p_world[:3] = world-frame position of the point

Inverting a Transformation

The inverse of a homogeneous transformation T is NOT just the matrix inverse (expensive). Because R is orthogonal, R^-1 = R^T. Therefore:

This is the "undo" transformation: if T maps body-frame coordinates to world-frame, then T^-1 maps world-frame back to body-frame. Computing it is just a transpose + multiply — far cheaper than general matrix inversion.

Chapter 6: DH Parameters

A robot arm is a chain of rigid links, connected by joints. Each joint adds one degree of freedom (for a revolute joint, one rotation angle θ). To compute the end-effector pose, you need to chain together the transformations of all n links. How do you parameterize this chain systematically?

The answer used by most robotics textbooks: the Denavit-Hartenberg (DH) convention, published in 1955. DH provides a systematic way to assign coordinate frames to robot links and express the transformation between consecutive frames using exactly four parameters per joint.

The Four DH Parameters

For each link i, four parameters describe the geometry:

For a standard revolute robot arm (like most industrial arms), a_i and α_i are fixed constants (determined by the arm's physical design), d_i are fixed offsets, and θ_i are the joint angles — the variables you control.

The DH Transformation Matrix

Each link contributes one 4×4 transformation matrix. The DH matrix for link i is the composition of four elementary transformations:

In explicit form:

Where c and s abbreviate cos and sin. This single matrix captures all four DH parameters in one expression.

python
import numpy as np

def dh_matrix(theta, d, a, alpha):
    """Standard DH transformation matrix for one link."""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [ct,  -st*ca,  st*sa,  a*ct],
        [st,   ct*ca, -ct*sa,  a*st],
        [0,    sa,     ca,     d   ],
        [0,    0,      0,      1   ]
    ])

# 2-link planar arm: both links length 1, no twist or offset
# DH table: theta=joint angle, d=0, a=1, alpha=0 for each link
def fk_2link(theta1, theta2):
    T1 = dh_matrix(theta1, 0, 1.0, 0)   # frame 0→1
    T2 = dh_matrix(theta2, 0, 1.0, 0)   # frame 1→2
    T = T1 @ T2                            # frame 0→2
    return T[:3, 3]  # end-effector xyz position

Chapter 7: Forward Kinematics — Showcase

We now have all the ingredients: link geometry (DH parameters), transformation matrices, and the chain rule T = T₁T₂...Tₙ. Forward kinematics is the computation that takes a vector of joint angles and returns the world-frame pose of the end-effector. Let's build it and watch it move.

The Forward Kinematics Formula

For an n-joint chain, the transformation from world frame to end-effector frame is:

Each T_i(θ_i) is the DH matrix for link i, which depends only on θ_i (for a revolute joint). The position of the end-effector in world coordinates is the last column of T_0→n (minus the last row). The orientation of the end-effector is the upper-left 3×3 submatrix.

Key insight: the elbow position is T₁(θ₁)[last column]. The wrist position is T₁T₂(θ₁,θ₂)[last column]. Each intermediate frame position is the last column of the partial product up to that link. This makes computing the full arm pose (all joint positions) efficient: just accumulate the product left to right.

Computing Joint Positions Step by Step

Let's trace through the math for a 2-link planar arm (both links of length 1, all in the xy-plane, so α_i = 0, d_i = 0):

python
import numpy as np

def forward_kinematics_planar(thetas, link_lengths):
    """
    Compute positions of all joints for a planar n-link arm.
    Returns list of (x,y) positions: [base, joint1, joint2, ..., end-effector]
    """
    positions = [(0.0, 0.0)]    # base always at origin
    T = np.eye(4)               # start with identity

    for theta, L in zip(thetas, link_lengths):
        # DH matrix for planar link: alpha=0, d=0
        c, s = np.cos(theta), np.sin(theta)
        Ti = np.array([
            [c, -s, 0, L*c],
            [s,  c, 0, L*s],
            [0,  0, 1,   0],
            [0,  0, 0,   1]
        ])
        T = T @ Ti             # accumulate: frame 0→current
        x, y = T[0,3], T[1,3]  # joint position = last column
        positions.append((x, y))

    return positions

# Example: 3-link arm, links of length [1, 1, 0.5]
thetas = [np.radians(30), np.radians(-60), np.radians(20)]
positions = forward_kinematics_planar(thetas, [1, 1, 0.5])
print("Joint positions:", positions)
# [(0, 0), (0.866, 0.5), (1.116, 0.067), (1.295, 0.238)]
# Each entry is the world-frame position of that joint.

Notice the data flow: joint angles (θ₁, θ₂, θ₃) go in, Cartesian positions (x, y) come out. The intermediate state at each step is a 4×4 matrix — the accumulated transformation from frame 0 to the current frame. The end-effector position is just T[0:3, 3] of the final accumulated matrix.

Chapter 8: Kinematic Trees

A robot arm is a chain: each link has exactly one parent and at most one child. But a human hand is not a chain — it has five fingers, each a chain of its own, all branching from the palm. A quadruped robot has four legs. A humanoid has two arms, two legs, and a head. These are kinematic trees.

Trees vs Chains

In a kinematic chain: T_0→n = T₁ · T₂ · ... · Tₙ. Each frame has exactly one parent. Forward kinematics is a single matrix product along the chain.

In a kinematic tree, each node has one parent but can have multiple children. To compute the pose of any end-effector, you trace the path from the root (base) to that end-effector — which is still a chain, just a sub-chain of the tree. Forward kinematics for a tree is just a depth-first traversal: accumulate T matrices down each branch.

Handling Loops: Closed Kinematic Chains

Some mechanisms form loops — not trees. A parallel robot (like a Stewart platform — the flight simulator base) has multiple chains connecting base to end-effector simultaneously. Closed kinematic chains are harder: the loops introduce kinematic constraints — algebraic equations that the joint angles must satisfy. Treating a closed chain as a tree and ignoring the loop constraints gives wrong kinematics.

LaValle's treatment focuses on open chains (no loops). Closed chains require specialized methods (constraint solving, reduced configuration spaces) that are beyond Chapter 3's scope.

Chapter 9: Connections

Chapter 3 is the geometric foundation for everything in Part II. Let's trace where each concept goes and what it unlocks.

What This Chapter Unlocks

The Jacobian: Preview

Forward kinematics gives x = f(θ) — end-effector position as a function of joint angles. The Jacobian J = ∂f/∂θ is the derivative of this function — it maps joint angular velocities θ̇ to end-effector velocities ẋ = J(θ)θ̇. The Jacobian is central to velocity control, singularity analysis, and trajectory optimization (Chapter 14). It's derived directly from the DH chain of Chapter 3.

Software Implementations

The algorithms of Chapter 3 are implemented in virtually every robotics library:

Comparison: Euler Angles vs Quaternions vs Rotation Matrices

Where to Go Next

"The configuration space is probably the most important concept in all of motion planning. Everything else depends on understanding what this space is and how to work with it."
— Steven LaValle, Planning Algorithms (2006), Ch 4 introduction