The Configuration Space

Chapter 1: Topology — Why Angles Aren’t Numbers

Here is a quiet trap that breaks motion planners if you ignore it. A revolute joint can rotate through angle θ. You might represent θ as a real number, say θ ∈ [0, 2π). But wait — is θ = 0° the same configuration as θ = 360°? Yes, of course. They’re the same physical orientation. But on the number line, 0 and 2π are different points, 6.28 apart. If your planner treats θ as a number in [0, 2π), it thinks the path from θ = 0.01 to θ = 2π − 0.01 must traverse the entire interval — a 6.27-radian journey — when there’s a 0.02-radian shortcut right across the boundary.

The problem is that the number line is the wrong topological space for angles. The right space is the circle, written S¹. On a circle, the two “endpoints” 0 and 2π are the same point, and the short path across that seam is perfectly valid. This is not a technicality — it changes which paths exist, which are shortest, and whether a planner finds them at all.

Topological spaces and open sets

A topological space is a set X together with a collection of “open sets” satisfying three axioms: the empty set and all of X are open; any union of open sets is open; any finite intersection of open sets is open. This seems abstract, but the open sets encode the neighborhood structure — they say which points are “close to” which other points, without needing a metric.

In R, the open sets are unions of open intervals (a, b). In contrast, [0, 1] is a closed set (it contains its boundary points 0 and 1). The interval [0, 1) is neither open nor closed — it contains the boundary point 0 but not 1. This distinction matters for C-space because joint limits create boundaries, and planners must handle them correctly.

Three properties of topological spaces that matter for planning

Not all topological spaces are equal for motion planning purposes. Three key properties determine what kinds of planners will work:

1. Hausdorff property: any two distinct points can be separated by disjoint open sets. Manifolds are Hausdorff. This ensures that two different robot configurations are genuinely distinct and cannot be confused by the topology. Without the Hausdorff property, a sequence of configurations could converge to two different limits simultaneously — a nightmare for any convergence-based planner.

2. Second countability: there is a countable basis for the topology (a countable collection of open sets such that every open set is a union of basis sets). This ensures that C-space is not “too large” for measure-theoretic arguments. All the manifolds in this chapter — Sⁿ, Tⁿ, SO(3), SE(3) — are second countable. This property underpins probabilistic completeness arguments for sampling-based planners: the space can be approximated arbitrarily well by a finite sample.

3. Paracompactness: every open cover has a locally finite refinement. This is a technical condition that allows for the existence of smooth partitions of unity, which are used to patch together local constructions (like local coordinate charts) into global ones. All manifolds are paracompact. For planning, this matters for gradient-based methods: the ability to do calculus globally on C-space requires paracompactness.

Continuity without a ruler

In topology, a function f: X → Y is continuous if the preimage of every open set in Y is open in X. This generalizes the calculus definition (“close inputs give close outputs”) to spaces where “close” is defined by open sets rather than by a metric. For robot motion planning, this means: a motion is a continuous path if small changes in time give small changes in configuration. In C-space, this is exactly the requirement that the path γ: [0,1] → C_free is a continuous function.

The path γ must be continuous: there should be no teleportation. When γ stays within C_free — every point on the path is collision-free — we have a valid motion plan. The fundamental planning question is: does such a γ exist, connecting q_initial to q_goal? This is the path existence problem, and its answer depends entirely on the topology of C_free.

Four spaces every roboticist must know

The crucial fact about S¹: it is compact (bounded and closed) but has no boundary. You can walk along a circle forever without reaching an edge. A number line has neither property. A closed interval [a, b] is compact but DOES have two boundary points. S¹ is the unique 1-manifold that is compact and boundary-free.

Compact vs non-compact: what it means for paths

A space is compact if every open cover has a finite subcover — the topological generalization of “closed and bounded.” For motion planning, compactness has two practical consequences. First, compact C-spaces have a finite diameter: there is a maximum possible path length between any two configurations. This means planners can reason about worst-case exploration distances. Second, on a non-compact space like Rⁿ, a path can escape to infinity; on a compact space like Tⁿ or Sⁿ, it cannot.

S¹ is compact (it is closed and bounded as a subset of R²). R is not compact (it is unbounded). T² is compact (product of compact spaces is compact). SE(2) = R²×S¹ is not compact (the R² factor is unbounded). In practice, we always restrict to a bounded workspace, making the effective C-space compact — but the underlying topology determines whether the boundaries are “hard walls” (joint limits, manifolds-with-boundary) or “wrap-arounds” (freely rotating joints, compact manifolds).

The distance problem: metrics on curved spaces

On a manifold, we need a distance function to measure how far apart two configurations are. On Rⁿ, Euclidean distance works. On S¹, the correct distance between angles θ₁ and θ₂ is the length of the shorter arc:

This is the geodesic distance on S¹: the length of the shortest path between the two points staying on the circle. For T² = S¹×S¹, the natural distance is the product metric: d((θ₁, φ₁), (θ₂, φ₂)) = √(d_S¹(θ₁, θ₂)² + d_S¹(φ₁, φ₂)²). For planners like RRT that need nearest-neighbor queries, using the right distance function is critical for finding nearby configurations across identification boundaries.

Homeomorphism: when two spaces are “the same”

A homeomorphism is a continuous bijection with a continuous inverse. If such a map exists between X and Y, we say X and Y are homeomorphic (written X ≅ Y) — they are topologically identical. The open interval (0, 1) ≅ R via f(x) = tan(π(x − ½)). The circle S¹ ≅ any closed curve without self-intersections. The classic punchline: a donut is homeomorphic to a coffee cup — both have exactly one hole. You can continuously deform one into the other.

For C-space, homeomorphism tells us when two representations are equivalent. If we parameterize a revolute joint as [0, 2π) versus as S¹, these are NOT homeomorphic — [0, 2π) has a boundary point (the gap at θ = 0/2π), while S¹ does not. A planner using [0, 2π) will refuse to plan across that gap. A planner using S¹ handles it naturally.

Manifolds: the smooth spaces robots live in

The spaces we care about in robotics are all manifolds: topological spaces that locally look like Rⁿ. Zoom in close enough to any point on S¹, and it looks like a line segment. Zoom in on any point on S², and it looks like a flat patch of R². The global topology can be complicated (sphere, torus, projective space), but locally everything is flat. This local flatness is what lets us do calculus on manifolds — and it is why motion planners can work with infinitesimally small steps even in curved C-spaces.

A manifold has a dimension equal to the dimension of the local Euclidean neighborhood. S¹ is a 1-manifold (locally like R). S² is a 2-manifold (locally like R²). SE(3) is a 6-manifold. A key fact: a manifold cannot have boundaries unless we explicitly declare them. S¹ and T² are closed manifolds (compact, no boundary). [0, 1] is a manifold with boundary. Revolute joints with mechanical stops are modeled as [a, b] (manifold with boundary); unconstrained revolute joints are S¹ (closed manifold).

Connectedness: can the robot get anywhere?

A topological space X is connected if it cannot be partitioned into two disjoint, non-empty open sets. Intuitively: you can travel between any two points. C_free may or may not be connected. If C_free is disconnected, there is no continuous path from q_initial to q_goal if they are in different connected components — the planning problem is infeasible.

A stronger notion is path-connectedness: X is path-connected if for any two points p, q ∈ X, there exists a continuous path γ: [0,1] → X with γ(0)=p and γ(1)=q. For “nice” spaces (manifolds, which all our C-spaces are), connectedness and path-connectedness are equivalent. So for robotics, we can use them interchangeably.

A planner like BFS or RRT implicitly tests path-connectedness: if the planner fails to find a path after exhaustive search, it is reporting that q_initial and q_goal are in different connected components of C_free. Proving that no path exists (rather than merely failing to find one) requires completeness guarantees — a major theme in Chapter 5.

Tangent spaces and velocities on manifolds

At any point q on a smooth manifold, the tangent space T_qM is the set of all possible velocity vectors at q — the directions in which a path through q can be moving at that instant. For Rⁿ, the tangent space at every point is just Rⁿ itself (all directions are possible). For S¹, the tangent space at each point is a 1D line tangent to the circle at that point.

Why do tangent spaces matter for planning? A path through C-space is a smooth map γ: [0,1] → C. Its derivative ˙γ(t) is a tangent vector in T_γ(t)C. For a robot with nonholonomic constraints, only certain tangent directions are admissible — the robot cannot move in arbitrary directions in C-space. Planning must stay within the admissible tangent cone at each point. For holonomic systems, all tangent directions are admissible, and any smooth path through C_free is a valid plan.

For SE(2), the tangent space at any configuration (x, y, θ) is 3-dimensional: instantaneous rates of change (˙x, ˙y, ˙θ). For a differential-drive robot, the nonholonomic constraint says ˙x sin θ − ˙y cos θ = 0 — this restricts the admissible tangent vectors to a 2-dimensional subspace at each configuration (forward/backward and turning). The admissible subspace varies continuously as θ changes, creating the curved structure of nonholonomic motion.

Chapter 2: Building Manifolds by Identification

How do we construct spaces like S¹? One elegant method is identification (also called “gluing”). Start with a simple, familiar space — like an interval or a square — and declare that certain points are “the same.” The resulting quotient space can have a completely different topology, even though it was built from something simple.

Example: take the unit interval [0, 1] and declare 0 ∼ 1 (zero and one are identified). Geometrically, you’re bending the interval into a loop and gluing the endpoints together. The result is S¹. The identification creates the “wrap-around” topology that makes S¹ different from [0, 1].

The square manifold zoo

The Asteroids game: living on a torus

LaValle uses a wonderful analogy from his actual text: the flat torus T² looks exactly like the universe in the classic Asteroids arcade game. When your spaceship flies off the right edge of the screen, it reappears on the left. When it flies off the top, it reappears at the bottom. The screen is a square with opposite edges identified — which is exactly the definition of T² = S¹×S¹.

A 2-link robot arm, where each joint can rotate fully 360°, has two angles θ₁ and θ₂. Each angle lives on S¹. The pair (θ₁, θ₂) lives on T² = S¹×S¹. This is NOT just R². If you plan on R² instead of T², you will miss paths that cross the θ = 0/2π boundary for either joint — topologically valid shortcuts that are physically equivalent to small rotations.

The flat torus vs the embedded torus: a crucial distinction

There are two ways to think about T². The embedded torus is the donut surface in R³: {(x,y,z) : (√(x²+y²)−R)² + z² = r²}. The flat torus is the square [0,1]×[0,1] with opposite edges identified. These are homeomorphic (same topology), but they are NOT isometric (they have different metrics).

The flat torus is what we actually want for C-space. On the flat torus, distances are computed with the product metric on S¹×S¹: d((θ₁,φ₁),(θ₂,φ₂)) = √(d_circ(θ₁,θ₂)² + d_circ(φ₁,φ₂)²). On the embedded torus, the inner circle (closer to the central axis) has a shorter circumference than the outer circle — the metric is not uniform. Using the embedded torus metric would incorrectly penalize configurations where the robot is near certain joint angles just because of how the torus happens to sit in 3D space, which has no physical meaning.

Why this matters for real planners

If you implement a path planner for a 2-link arm and you store the configuration as two floating-point angles in [0, 2π), then when θ₁ is at 0.05 rad and the planner wants to decrease it to 2π − 0.05 rad (a tiny clockwise step), the planner sees a jump of nearly 2π — a giant step in its distance function. The correct representation stores (θ₁, θ₂) on T² with a distance metric that knows the boundary is identified. The distance between 0.05 and 2π − 0.05 is 0.1 rad, not 6.18 rad.

Product spaces: building higher-dimensional C-spaces from pieces

Most robot C-spaces are product manifolds: you take the C-space factors for each independent DOF and multiply them together. The product X×Y is just the set of all pairs (x, y) with x ∈ X and y ∈ Y, with the product topology (open sets are products of open sets). This is exactly how R³ = R×R×R works, but now applied to non-Euclidean factors.

A 3-link arm where link 1 can fully rotate (S¹), link 2 has a joint limit (interval [−π/2, π/2]), and link 3 can fully rotate (S¹) has C-space S¹×[−π/2, π/2]×S¹. This is a 3-manifold with boundary (the middle factor has endpoints). Path planners on this space must handle the boundary correctly: configurations at θ₂ = ±π/2 are physically reachable but the joint cannot move further.

Worked example: the 2-link arm on T²

Let’s work through a concrete planning scenario to feel the difference. A 2-link arm is at configuration q_I = (0.1, 0.1) (both joints near 0 radians). The goal is q_G = (6.1, 6.1) (both joints near 2π radians — barely past a full revolution, physically almost identical to q_I).

On R²: the Euclidean distance from q_I to q_G is √((6.0)² + (6.0)²) ≈ 8.49. The planner plans a long sweeping path across most of the configuration space, rotating both joints nearly 360°.

On T²: the toroidal distance is computed as min(|Δθ₁|, 2π−|Δθ₁|) for each joint separately. For both joints, |Δθ| = 6.0, and 2π−6.0 ≈ 0.28. So the toroidal distance is √((0.28)² + (0.28)²) ≈ 0.40. The planner finds the short path that crosses the seam — a tiny movement of about 0.28 radians per joint — instead of the 360° detour.

The ratio is 8.49 / 0.40 ≈ 21x. A planner using the wrong topology finds a path 21 times longer than necessary in this case. In surgical robot applications with cable-driven joints, this difference directly translates to excess cable wear, slower movements, and potentially reaching joint limits unnecessarily.

CW complexes: building spaces from cells

Another way to build topological spaces is as CW complexes: start with a set of 0-cells (points), attach 1-cells (line segments, glued at endpoints), attach 2-cells (disks, glued at boundaries), and so on. S¹ as a CW complex: one 0-cell (a point) and one 1-cell (an arc, with both endpoints identified to the 0-cell). T² as a CW complex: one 0-cell, two 1-cells (the two circle factors, one in each direction), one 2-cell (the square, with boundary identified to the 1-skeleton).

This cell decomposition is useful for planning because it provides a finite combinatorial structure that approximates the continuous manifold. Cell decomposition planners (Chapter 6 of LaValle) work by decomposing C_free into cells and building a roadmap on the cell adjacency graph. The topology of the cells and how they fit together determines whether the planner correctly handles the manifold structure.

The boundary of a manifold-with-boundary

Joint limits impose boundaries on C-space. A revolute joint limited to [−π, π] has C-space [−π, π] — a manifold-with-boundary. The boundary consists of the two endpoints {−π, π}. At a boundary configuration, the joint cannot move further in the constrained direction: the path must stay in the interior or return from the boundary.

Planners that sample C-space must handle boundaries. A configuration sampled uniformly from [a, b]×S¹ might land exactly at x = a or x = b — a valid configuration, but where one direction of motion is blocked. The planner must not try to extend the tree “through” the boundary. In practice, sampled configurations are checked against joint limits, and steering functions clamp to the boundary when they would overshoot.

Higher identifications: the Klein bottle and RP²

For completeness, let’s briefly visit the non-orientable identifications from the table. The Klein bottle is constructed from a square by identifying the left-right edges normally ((0,y)∼(1,y)) but identifying the top-bottom edges with a flip ((x,0)∼(1−x,1)). The result is a closed, non-orientable surface that cannot be embedded in R³ without self-intersection. It has no C-space role in standard robotics but appears in topology textbooks as a canonical example.

The real projective plane RP² identifies both pairs of opposite edges with flips. It is also non-orientable, also cannot be embedded in R³, and π₁(RP²) = Z₂. Its 3D analog RP³ = S³/∼ (antipodal identification on S³) IS the topology of SO(3), as we discuss in Chapter 5. Non-orientable spaces are rare in robot C-spaces (all the common ones — SE(2), SE(3), Tⁿ — are orientable), but the identification technique that generates them is the same one that generates all the spaces we do use.

Implementing toroidal distance correctly in code

Here is a common implementation mistake and its fix. In an RRT for a 2-link arm, we need to find the nearest existing node to a randomly sampled configuration. If we use Euclidean distance on raw angles, we get wrong nearest-neighbors near the wrap-around boundary.

python
import numpy as np

# WRONG: Euclidean distance ignores wrap-around
def bad_distance(q1: np.ndarray, q2: np.ndarray) -> float:
    return np.linalg.norm(q1 - q2)  # ||(θ₁,θ₂) - (φ₁,φ₂)||₂
    # BUG: distance(0.1, 6.1) = 6.0, should be ~0.28

# CORRECT: Toroidal distance (product metric on T²)
def circle_dist(a: float, b: float) -> float:
    """Geodesic distance between two angles on S¹."""
    diff = abs(a - b) % (2 * np.pi)
    return min(diff, 2 * np.pi - diff)

def torus_distance(q1: np.ndarray, q2: np.ndarray) -> float:
    """Correct geodesic distance on T^k for k revolute joints."""
    return np.sqrt(sum(circle_dist(q1[i], q2[i])**2
                       for i in range(len(q1))))

# Example: q1=(0.1, 0.1), q2=(6.1, 6.1) (both just past 0, physically close)
q1 = np.array([0.1, 0.1])
q2 = np.array([6.1, 6.1])

d_bad    = bad_distance(q1, q2)    # ≈ 8.49 — wildly wrong
d_correct = torus_distance(q1, q2)  # ≈ 0.40 — correct
# 21x difference in nearest-neighbor priority for RRT!

Chapter 3: Fundamental Group — Counting Holes

Two spaces can look similar — both are 2D, both are smooth — but have fundamentally different loop structures. A disk has no holes; a washer has one. You can shrink any loop on a disk to a point. On a washer, a loop that goes around the hole cannot be shrunk without cutting it. How do we make this precise? The answer is the fundamental group π₁(X).

Fix a basepoint p ∈ X. A loop based at p is a continuous map γ: [0,1] → X with γ(0) = γ(1) = p. Two loops are homotopic if one can be continuously deformed into the other while keeping the basepoint fixed. Homotopy is an equivalence relation — it partitions the set of all loops into classes. The fundamental group π₁(X) is the set of these homotopy classes, with group operation “traverse one loop then the other.”

The formal definition of π₁

Fix a basepoint p ∈ X. The group operation on homotopy classes [γ₁] and [γ₂] is concatenation: first traverse γ₁ at double speed, then γ₂ at double speed — both completing in the unit time interval [0,1]. The inverse of [γ] is the reverse path γ⁻¹(t) = γ(1−t). The identity is the constant path at p.

Two key facts: (1) π₁ is well-defined (the group axioms hold up to homotopy), and (2) for path-connected spaces, changing the basepoint gives an isomorphic group (so the choice of p doesn’t matter for the algebraic structure, only for concrete element labeling). For all the spaces we care about, we can pick any convenient basepoint.

Computing π₁ for the spaces we care about

R² (or any Rⁿ): Simply connected. Every loop can be continuously shrunk to the basepoint through the interior of Rⁿ. π₁(Rⁿ) = {e} — the trivial group with one element.

S¹ (the circle): A loop that winds once around S¹ cannot be shrunk — there is no 2D interior to move through. Winding once counterclockwise and winding once clockwise are different classes. Winding twice counterclockwise is yet another class. In fact, each integer winding number gives a distinct homotopy class, and combining two loops adds their winding numbers. π₁(S¹) = Z (the integers under addition).

T² = S¹×S¹ (the torus): Two independent circles, two independent winding numbers. π₁(T²) = Z×Z = Z². A loop that winds 3 times in the θ₁ direction and −2 times in the θ₂ direction belongs to the class (3, −2) ∈ Z².

RP² (real projective plane, relevant for SO(3)): Strange behavior. Winding twice is homotopic to winding zero times! π₁(RP²) = Z₂ = {0, 1} (the integers mod 2). The same phenomenon occurs in RP³, which is homeomorphic to SO(3).

Why π₁ matters for planning

If π₁(C_free) is non-trivial, there are multiple homotopy classes of paths between two configurations. A planner that finds one path may have found the unique class — or it may have found the expensive class while missing a shorter one. On a cylinder R×S¹ with an obstacle spanning the cylinder, the only path from q_I to q_G may go “over the seam” — crossing the S¹ identification boundary. A planner that treats the S¹ direction as a number line will declare the path impossible.

The plate trick (also called the Dirac belt trick) is the physical manifestation of π₁(SO(3)) = Z₂. Hold a plate flat on your palm and rotate it 360° around a vertical axis: your arm is twisted. Continue to 720°: your arm untwists to its original position. A 360° rotation of a rigid body is not contractible to no rotation, but a 720° rotation IS. This is a topological fact about RP³, and it has real consequences for cable-routing and tether management in robot systems.

Simply connected spaces: when π₁ = trivial simplifies planning

When C_free is simply connected (π₁(C_free) = {e}), every two paths with the same endpoints are homotopic. This means there is essentially one homotopy class of paths between any two configurations. A planner can find “the” path without worrying about missing a topologically distinct alternative. This is the common case for many practical robot systems: a robot arm in free space with no obstacles looping around any joint has simply connected C_free.

But simply connected does NOT mean easy to plan in. A simply connected space can have very narrow passages, very high dimension, very complex geometry. It just means the topology is not adding extra difficulty beyond the geometry. The planning algorithms of Chapters 5 and 6 are designed to work in both simply connected and non-simply-connected C_free; the topology affects whether they are complete and what guarantees they can provide.

Homotopy classes and planners

If C_free has a non-trivial fundamental group, there are multiple homotopy classes of paths from q_I to q_G. This means: there are qualitatively different routes that cannot be continuously deformed into each other. One route might go “above” an obstacle and one “below,” where the obstacle is a hole in C_free.

Standard planners (RRT, PRM) are single-homotopy-class planners. They find one path and stop. If the first path they find is in the expensive homotopy class, they miss the shorter path in a different class. Homotopy-aware planners — an active research area — explicitly track which class each candidate path belongs to, ensuring they explore all classes before declaring optimality.

LaValle’s Figure 4.8 gives the canonical example: a cylinder R×S¹ with a wall-like obstacle that extends all the way across the S¹ direction at a fixed height. The only path from bottom to top must cross the S¹ identification boundary. π₁ = Z tells us there are infinitely many homotopy classes — each number of times the path wraps around S¹ gives a different class. The planner must understand this topology, or it will miss the path.

Higher homotopy groups: π₂, π₃

The fundamental group π₁ counts distinct classes of loops. Higher homotopy groups count distinct classes of higher-dimensional maps. π₂(X) counts equivalence classes of maps S² → X (2-sphere mappings). For S² itself: π₂(S²) = Z, meaning 2-sphere maps into S² have a winding number, just as loop maps into S¹ do.

For robotics: π₂(SO(3)) = 0 (trivial), but π₃(SO(3)) = Z. This means that while all sphere-shaped configurations are contractible in SO(3), there are non-trivial 3-sphere families of rotations. This appears in the context of Hopf fibrations and has connections to spin representations in quantum mechanics — fascinating mathematics, but beyond the needs of most motion planners.

The key practical lesson: for the motion planning problems in LaValle’s book, we need π₁ (loop structure, path connectivity) and the connectedness of C_free. Higher homotopy groups appear in more advanced topics like topological complexity of motion planning (Farber’s TC invariant) and configuration spaces of many-robot systems.

Van Kampen’s theorem: computing π₁ of compound spaces

For product spaces X×Y, the fundamental group is the product: π₁(X×Y) = π₁(X) × π₁(Y). This is why π₁(T²) = π₁(S¹×S¹) = Z×Z. For more complex constructions — spaces formed by gluing two spaces along a common subspace — the Seifert-van Kampen theorem gives a way to compute π₁ from the pieces.

The practical upshot: once you know the C-space decomposition into product factors, you can compute π₁(C) by multiplying the π₁s of each factor. A k-link arm with all revolute joints has C-space T^k = (S¹)^k, and π₁(T^k) = Z^k. The planner needs to be aware of k independent “winding number” choices for the path — 2^k qualitatively distinct path classes to potentially explore.

Chapter 4: C-Space for 2D Bodies — SE(2)

Time to get concrete. What exactly is the C-space for a rigid body that can move freely in a 2D plane? We need to build the mathematical structure, not just name it. The answer comes from understanding rigid motions as a matrix Lie group.

A rigid motion in 2D preserves distances and orientations. It consists of a rotation and a translation. The rotation part is a 2×2 matrix R(θ); the translation part is a 2D vector (x, y). LaValle derives the structure bottom-up, starting from the most general invertible matrices.

Deriving SO(2) from first principles

Start with GL(2): the set of all 2×2 invertible real matrices. This has 4 free entries and forms a 4-dimensional manifold (a Lie group). That’s too general — GL(2) includes stretching, shearing, and reflection, not just rotation.

Impose the constraint AA^T = I (orthogonality: columns are orthonormal). For A = [[a, b], [c, d]], this gives three scalar equations: a²+b²=1 (first column unit), c²+d²=1 (second column unit), ac+bd=0 (columns orthogonal). Three constraints on four free parameters — by the implicit function theorem, the solution set is generically a 4−3=1 dimensional manifold. The result is O(2): the orthogonal group.

O(2) has two connected components, separated by the determinant. When det(A) = +1, the matrix is a pure rotation. When det(A) = −1, the matrix is a reflection. These two sets are topologically disconnected: you cannot continuously deform a rotation into a reflection while staying in O(2). Impose det(A) = 1 to isolate the rotations: the result is SO(2), the special orthogonal group.

The explicit parameterization: for any θ, the matrix R(θ) = [[cos θ, −sin θ], [sin θ, cos θ]] satisfies R(θ)R(θ)^T = I and det(R(θ)) = cos²θ + sin²θ = 1. Conversely, any element of SO(2) has this form for some θ. The map θ → R(θ) is a bijection S¹ → SO(2) that is continuous and has a continuous inverse — a homeomorphism. We have proven SO(2) ≅ S¹.

SE(2): combining rotation and translation

A 2D rigid body can both rotate (by θ ∈ S¹) and translate (by (x,y) ∈ R²). The Special Euclidean group SE(2) combines these: C = R² × S¹. Its elements are represented as 3×3 homogeneous transformation matrices:

Why homogeneous coordinates? Because composing rigid motions becomes matrix multiplication: T₁ · T₂ applies T₂ first, then T₁. A point p = (p_x, p_y)^T in the robot’s body frame transforms to T · (p_x, p_y, 1)^T in the world frame. This is clean, composable, and directly implementable.

SE(2) is a 3D manifold

SE(2) has dimension 3: two translation DOF (x, y ∈ R) and one rotation DOF (θ ∈ S¹). The topology is R²×S¹: a cylinder (R×S¹) extended into a second translational direction. It is not compact — translations can go to infinity. Its fundamental group is π₁(R²×S¹) = π₁(S¹) = Z, because π₁(R²) is trivial.

Worked example: composing SE(2) transforms

Let’s trace through a concrete SE(2) computation. A robot starts at the origin facing right: T₁ = se2(0, 0, 0). It drives forward 3 meters (in the +x direction of its body frame): this is a translation of (3, 0) in the robot’s body frame. Then it turns left 90° (counterclockwise). Then it drives forward 2 meters again.

Step 1: T_forward3 = se2(3, 0, 0). After applying this to T₁: T_{after_forward} = T₁ · T_forward3 = se2(3, 0, 0). Robot is now at (3, 0) facing right.

Step 2: T_{turn_left} = se2(0, 0, π/2). After turning: T_{after_turn} = T_{after_forward} · T_{turn_left} = se2(3, 0, π/2). Robot is at (3, 0) facing up.

Step 3: T_forward2 = se2(2, 0, 0) (2 meters forward in current body frame direction). Apply in robot’s current frame: T_final = T_{after_turn} · T_forward2. Since the robot faces π/2 (up), driving “forward” in body frame (+x body direction) moves it in the world’s +y direction: T_final = se2(3, 2, π/2). Robot ends at (3, 2) facing up.

The key: we always compose transforms as T_{world→new_pose} = T_{world→old_pose} · T_{old_pose→new_pose}. The relative transform is always expressed in the robot’s current frame. This left-multiplication by the old pose correctly handles the rotation, which is why the 2m forward step went to (3,2) and not (5,0).

SO(2) as a complex number: the cleanest representation

There is an even cleaner way to think about SO(2). Instead of a 2×2 matrix with the constraint AA^T = I, det(A) = 1, we can use a unit complex number: z = e^iθ = cos θ + i sin θ, with |z| = 1. Multiplication of two unit complex numbers corresponds to adding their angles: e^iα · e^iβ = e^i(α+β). This is rotation composition. The constraint |z| = 1 is one scalar equation on two reals — confining z to S¹, which is exactly SO(2).

The same idea extends to 3D: SO(3) uses unit quaternions (unit elements of the 4D quaternion algebra), which we explore in Chapter 5. And in both cases, the algebraic structure (complex numbers, quaternions) exactly captures the rotation group structure, making these parameterizations natural rather than arbitrary.

What makes SE(2) a Lie group

SE(2) is not just a manifold — it is a Lie group: a group that is also a smooth manifold, where the group operations (multiplication and inversion) are smooth maps. The Lie group structure means SE(2) has a natural notion of “tangent space at the identity,” called the Lie algebra se(2). For motion planning, this matters because it gives us a principled way to do interpolation (moving from one configuration to another along a geodesic) and to define velocities (elements of the Lie algebra are instantaneous velocities).

The Lie algebra se(2) consists of 3×3 matrices of the form [[0, −ω, v_x], [ω, 0, v_y], [0, 0, 0]], where ω is the angular velocity and (v_x, v_y) is the linear velocity. The exponential map exp: se(2) → SE(2) converts instantaneous velocities into finite transformations. This is the foundation for screw theory and exponential coordinates, which appear throughout modern robot kinematics.

SE(2) and differential drive robots

A differential-drive robot (two powered wheels on a common axle) has three configuration variables (x, y, θ) but only two independently controllable motors — it cannot move sideways. This means the robot’s instantaneous velocity must satisfy the nonholonomic constraint: the velocity must be tangent to the direction the robot faces. Formally, ˙x sin θ − ˙y cos θ = 0 (no lateral motion).

This constraint limits which paths through SE(2) the robot can actually follow. Not all paths through C_free are physically realizable — only those whose tangent vectors satisfy the constraint at every point. This is the key distinction between holonomic systems (any path through C_free is realizable) and nonholonomic systems (only constraint-satisfying paths are realizable). The full C-space is still SE(2), but the reachable paths form a subset constrained by the robot’s dynamics.

The inverse and interpolation in SE(2)

For group-aware planners, we need the inverse of a SE(2) transform. If T = se2(x, y, θ) is the pose of the robot in the world, then T⁻¹ is the pose of the world in the robot’s frame. The formula:

In matrix form, since T = [[R, t], [0, 1]], we have T⁻¹ = [[R^T, −R^Tt], [0, 1]]. For SE(2), R^T = R(−θ) = [[cos θ, sin θ], [−sin θ, cos θ]], and −R^Tt = (−x cos θ − y sin θ, x sin θ − y cos θ).

For interpolating between two SE(2) configurations q₀ = (x₀, y₀, θ₀) and q₁ = (x₁, y₁, θ₁), the natural interpolation is linear for translation and shortest-arc for rotation:

where the third component uses the signed shortest arc from θ₀ to θ₁ (positive if counterclockwise, negative if clockwise). This gives a straight-line interpolation in (x,y) space with simultaneous shortest-arc rotation — the natural “straight line” in SE(2) for a holonomic robot.

python
import numpy as np

def se2_matrix(x: float, y: float, theta: float) -> np.ndarray:
    """SE(2) homogeneous transformation matrix.

    Args:
        x, y: translation (meters)
        theta: rotation (radians)
    Returns:
        3x3 homogeneous matrix T in SE(2)
    """
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [c, -s, x],
        [s,  c, y],
        [0,  0, 1]
    ], dtype=np.float64)

def compose_se2(T1: np.ndarray, T2: np.ndarray) -> np.ndarray:
    """Compose two SE(2) transforms: apply T2 first, then T1."""
    return T1 @ T2  # 3x3 matrix multiplication

def apply_se2(T: np.ndarray, point: np.ndarray) -> np.ndarray:
    """Apply SE(2) transform to a 2D point."""
    p_hom = np.array([point[0], point[1], 1.0])
    result = T @ p_hom
    return result[:2]  # drop homogeneous coordinate

# Example: robot at (3, 2) rotated 45 degrees
T = se2_matrix(x=3.0, y=2.0, theta=np.pi/4)

# A point at (1, 0) in robot frame
p_body = np.array([1.0, 0.0])
p_world = apply_se2(T, p_body)
# p_world ≈ [3.707, 2.707] — 45° rotation + translation

# Composition: first move 1m in x, then rotate 90°
T1 = se2_matrix(0, 0, np.pi/2)   # 90° rotation
T2 = se2_matrix(1, 0, 0)         # 1m translation in x
T_composed = compose_se2(T1, T2)   # rotate AFTER translate
# Result: translation 1m in x, THEN 90° — origin at (0,1) facing left

Chapter 5: SE(3) and Quaternions

A rigid body in 3D space has 6 DOF: three translations (x, y, z ∈ R³) and three rotations. The C-space is SE(3) = R³×SO(3). Translations are easy — R³ is R³. The challenge is SO(3): the group of 3D rotations.

The naive approach — three Euler angles (roll, pitch, yaw) — seems like it should work. Three angles, three DOF, done. But it fails in the most inconvenient way possible, exactly at the worst configurations. Understanding why forces us to find a better parameterization: quaternions.

First: deriving SO(3) by the same dimensional argument

We can run the same derivation as SO(2), but now in 3D. Start with GL(3): all 3×3 invertible matrices, a 9-dimensional manifold. Impose AA^T = I: six constraints (three unit-column constraints, three orthogonality constraints — the upper triangle of AA^T = I is 6 equations). This leaves a 9−6 = 3-dimensional manifold: O(3). Impose det(A) = 1: one more constraint cutting O(3) in half, giving SO(3), a 3-dimensional manifold.

The three dimensions of SO(3) correspond to three independent ways to rotate a rigid body in 3D (around each of three axes, for example). What is the topology of SO(3)? The key claim from LaValle Section 4.2.2: SO(3) ≅ RP³. Every unit quaternion h ∈ S³ maps to a rotation, and h and −h give the same rotation, so SO(3) = S³ / (h ∼ −h) = RP³. This identification is what makes SO(3) topologically non-trivial — and it is the source of gimbal lock.

The Euler angle failure: gimbal lock

Represent a 3D rotation as three successive rotations: yaw (around Z), then pitch (around Y), then roll (around X). This works generically, but at pitch = ±90°, the roll and yaw axes align. You lose one degree of freedom. No matter how you change roll or yaw independently, they both produce the same effect — rotation around the same axis. This is gimbal lock.

Gimbal lock is not just a numerical issue — it’s a topological fact. You cannot globally parameterize SO(3) with three angles without singularities. This follows from the Hairy Ball Theorem (you cannot comb a 3-sphere flat). Any chart on SO(3) using three real-valued angles must have at least one singular point. Euler angles have infinitely many (the entire set of ±90° pitch configurations).

Why we can't avoid the topology of SO(3)

The topological obstruction to gimbal-lock-free Euler angles is fundamental, not a matter of choosing a better angle convention. The Hairy Ball Theorem states that there is no continuous, non-vanishing tangent vector field on S². A related result (which we won’t prove here) states that SO(3) cannot be covered by a single coordinate chart without singularities. Any attempt to describe all of SO(3) using three continuous angles must have degenerate points — configurations where the mapping from angles to rotations is not locally invertible.

Different Euler angle conventions (ZYX, ZYZ, XYZ, and 9 others) all have singularities, just at different configurations. ZYX (yaw-pitch-roll) locks at pitch ±90°. ZYZ (aerospace convention) locks at zero pitch. There is no singularity-free 3-angle representation of SO(3). The minimum-overhead solution is unit quaternions: 4 floats with one quadratic constraint, no singularities, direct geodesic interpolation.

Quaternions: the right space for 3D rotations

A quaternion h = a + bi + cj + dk is a 4D number with three imaginary units i, j, k satisfying: i² = j² = k² = ijk = −1. The key identities are ij = k, jk = i, ki = j (and their negatives when reversed). A unit quaternion satisfies a² + b² + c² + d² = 1, which means it lives on the 3-sphere S³ ⊂ R⁴.

Quaternion algebra: the rules

Quaternion multiplication is associative but not commutative: in general, h₁ · h₂ ≠ h₂ · h₁. This non-commutativity directly reflects the non-commutativity of 3D rotations: rotating 90° around X then 90° around Y gives a different result from rotating 90° around Y then 90° around X. The quaternion algebra exactly captures this.

The conjugate of h = a + bi + cj + dk is h* = a − bi − cj − dk. For unit quaternions, h⁻¹ = h* (inverse = conjugate). This is analogous to how rotation matrices satisfy R⁻¹ = R^T. The product hh* = a²+b²+c²+d² = 1 for unit quaternions, confirming the norm is preserved.

To rotate a 3D vector v using quaternion h, embed v as a pure quaternion p = 0 + v_xi + v_yj + v_zk, then compute h · p · h*. The result is another pure quaternion whose imaginary part is the rotated vector. This formula is equivalent to applying the rotation matrix R(h), but works directly in quaternion space without expanding to a 3×3 matrix.

Unit quaternions parameterize 3D rotations via the axis-angle formula. For a rotation of angle θ around unit axis v = (v₁, v₂, v₃):

The corresponding 3×3 rotation matrix (LaValle eq. 4.20) is:

The double-cover: why SO(3) ≅ RP³

Here is the crucial topological fact (LaValle Figure 4.10): h and −h give the same rotation. The quaternion (−a, −b, −c, −d) describes the rotation of angle (2π−θ) around axis −v, which is physically identical to rotating θ around v. Every rotation corresponds to exactly two antipodal points on S³: {h, −h}.

So SO(3) is not S³ — it is S³ with antipodal points identified: S³/~ = RP³. The unit quaternion sphere is a double cover of SO(3). This identification has the fundamental group consequence we saw in Chapter 3: π₁(RP³) = Z₂, explaining the plate trick.

Let’s verify the double-cover concretely. For the rotation h = cos(θ/2) + sin(θ/2) · v:

• At θ = 0: h = 1 + 0 = (1,0,0,0). At θ = 2π: h = cos(π) + sin(π) · v = (−1, 0, 0, 0). These are antipodal on S³. Both represent the identity rotation. A continuous path of rotations from θ=0 to θ=2π lifts to a path from h=(1,0,0,0) to h=(−1,0,0,0) on S³ — NOT a loop on S³.
• At θ = 4π: h = cos(2π) + sin(2π) · v = (1,0,0,0). A path of rotations from 0 to 4π lifts to a loop on S³ that starts and ends at (1,0,0,0). This loop can be contracted to a point on S³. This is why 720° is contractible but 360° is not — the plate trick, in quaternion language.

SLERP: geodesics on SO(3)

Given two orientations represented as unit quaternions h₀ and h₁, the shortest rotation path between them is the geodesic on S³. This geodesic is parameterized by spherical linear interpolation (SLERP):

Or equivalently, if Ω = arccos(h₀ · h₁) is the angle between them on S³:

SLERP produces constant angular velocity along the shortest rotation path. For motion planning, it provides a natural steering function: given two configurations q_a = (t_a, h_a) and q_b = (t_b, h_b) in SE(3), we can interpolate translations linearly and rotations via SLERP to get a valid path through C-space (assuming no obstacles). This is used directly in RRT-Connect and similar bidirectional planners operating in SE(3).

SE(3): the full 6-DOF C-space

The C-space of a 3D rigid body is SE(3) = R³×SO(3). Since SO(3) ≅ RP³, we have SE(3) ≅ R³×RP³. This is a 6-dimensional manifold. Its elements are represented as 4×4 homogeneous transformation matrices:

where R is the 3×3 rotation matrix from the quaternion formula above, and (x, y, z) is the translation. Composition of rigid motions is again matrix multiplication. This is the standard format used by every robotic library: ROS uses SE(3) transforms, robotics simulators use SE(3) transforms, computer vision uses SE(3) transforms.

SE(3) in practice: the transform tree

A robot operating in 3D space maintains a transform tree: a directed graph of SE(3) transforms linking coordinate frames. Each node is a frame (world, robot base, each link, each camera). Each edge is a 4×4 T matrix. To compute the pose of the end-effector in world coordinates, you multiply the transforms along the path from world to end-effector: T_world→ee = T_world→base · T_base→link1 · … · T_{link(n-1)→ee}.

This chain of SE(3) multiplications is called forward kinematics — the topic of Chapter 6. For now, the key point is that each joint contributes one SE(3) transform parameterized by the joint’s configuration variable (angle for revolute, displacement for prismatic). The C-space of the entire chain is the product of the C-space factors for each joint.

Non-compactness and planning in SE(3)

SE(3) is not compact (translations can go to infinity). For planning a drone or autonomous vehicle, we operate in a bounded workspace — a compact subset of SE(3) — and the planner works within that subset. For a robot arm fixed to a base, the translations are constrained by the arm’s reach, effectively compactifying the reachable C-space. In practice, planners always work with bounded C-spaces, obtained by taking the physically meaningful subset of the full mathematical C-space.

The sampling distribution for sampling-based planners must respect the topology. For the translation part (R³), uniform sampling in a bounding box works fine. For the rotation part (SO(3) ≅ RP³), uniform sampling in SO(3) via uniform sampling on S³ (and then identifying antipodal points) gives the Haar measure — the unique rotation-invariant distribution on SO(3). To sample uniformly on S³, generate four independent Gaussians (a, b, c, d) ∼ N(0,1) and normalize: h = (a,b,c,d)/|(a,b,c,d)|. This is the standard method used in RRT and PRM implementations for SE(3) planning.

python
import numpy as np

def quat_multiply(h1: np.ndarray, h2: np.ndarray) -> np.ndarray:
    """Quaternion multiplication h1 * h2 (LaValle eq. 4.19).

    h = [a, b, c, d] where h = a + bi + cj + dk
    """
    a1, b1, c1, d1 = h1
    a2, b2, c2, d2 = h2
    return np.array([
        a1*a2 - b1*b2 - c1*c2 - d1*d2,   # a component
        a1*b2 + b1*a2 + c1*d2 - d1*c2,   # b (i) component
        a1*c2 - b1*d2 + c1*a2 + d1*b2,   # c (j) component
        a1*d2 + b1*c2 - c1*b2 + d1*a2,   # d (k) component
    ])

def axis_angle_to_quat(axis: np.ndarray, angle: float) -> np.ndarray:
    """Convert axis-angle to unit quaternion (LaValle eq. 4.21)."""
    axis = axis / np.linalg.norm(axis)  # ensure unit axis
    return np.array([
        np.cos(angle / 2),           # a = cos(θ/2)
        axis[0] * np.sin(angle / 2),  # b = v₁ sin(θ/2)
        axis[1] * np.sin(angle / 2),  # c = v₂ sin(θ/2)
        axis[2] * np.sin(angle / 2),  # d = v₃ sin(θ/2)
    ])

def quat_to_rotmat(h: np.ndarray) -> np.ndarray:
    """Quaternion to 3x3 rotation matrix (LaValle eq. 4.20)."""
    a, b, c, d = h
    return np.array([
        [2*(a**2+b**2)-1,  2*(b*c-a*d),    2*(b*d+a*c)  ],
        [2*(b*c+a*d),    2*(a**2+c**2)-1,  2*(c*d-a*b)  ],
        [2*(b*d-a*c),    2*(c*d+a*b),    2*(a**2+d**2)-1],
    ])

# Example: 90° rotation around Z axis
h_z90 = axis_angle_to_quat(np.array([0, 0, 1]), np.pi/2)
# h_z90 ≈ [0.707, 0, 0, 0.707] = cos(45°) + sin(45°)k

# Verify: h and -h give the same rotation matrix
R1 = quat_to_rotmat(h_z90)
R2 = quat_to_rotmat(-h_z90)
# np.allclose(R1, R2) → True  ← the double-cover in action

# Compose two rotations: 90° around Z, then 90° around X
h_x90 = axis_angle_to_quat(np.array([1, 0, 0]), np.pi/2)
h_composed = quat_multiply(h_x90, h_z90)  # Z first, then X
R_composed = quat_to_rotmat(h_composed)

def se3_matrix(t: np.ndarray, h: np.ndarray) -> np.ndarray:
    """SE(3) 4x4 homogeneous matrix from translation t and quaternion h."""
    T = np.eye(4)
    T[:3, :3] = quat_to_rotmat(h)
    T[:3,  3] = t
    return T

Comparing rotation representations

For motion planning, unit quaternions are almost always the right choice. They are singularity-free, overdetermined by only one constraint (easy to enforce by normalizing), support SLERP for geodesic interpolation, and have efficient multiplication formulas. Rotation matrices are excellent for applying transforms to points (matrix-vector multiply is fast). Euler angles should almost never appear inside a planner — only at the user interface for human-readable input/output.

Distance on SE(3) for planning

Planners like RRT need a distance function d(q₁, q₂) to choose which node in the tree to extend toward. On SE(3), the natural choice combines translation distance with rotation distance:

where d_SO(3)(R₁, R₂) = arccos((trace(R₁^TR₂)−1)/2) is the geodesic distance on SO(3), and α, β are weighting constants that depend on the problem scale (meters vs radians). Setting α and β correctly matters: if α >> β, the planner ignores rotations; if β >> α, it ignores translations. There is no universal correct weighting — it depends on the task.

In quaternion form, the geodesic distance is simpler to compute: d_SO(3)(h₁, h₂) = 2 arccos(|h₁ · h₂|), where the absolute value handles the double-cover (h and −h have zero distance to each other). The factor of 2 converts from S³ arc length to SO(3) arc length. This formula is the one to use in any planner operating in SO(3) or SE(3).

Uniform sampling on SO(3)

Sampling-based planners need to sample configurations uniformly at random from C-space. For Rⁿ, sampling uniformly in a bounding box is trivial. For SO(3), “uniform” means the Haar measure: the unique probability measure on SO(3) invariant under left and right multiplication by any fixed rotation. Sampling Euler angles uniformly from [0, 2π) does NOT give uniform rotations — you over-sample near the poles (gimbal-lock-adjacent regions).

The correct method: generate (a, b, c, d) with each component independently drawn from N(0,1), then normalize h = (a,b,c,d)/|(a,b,c,d)|. The resulting unit quaternions are uniform on S³ by the spherical symmetry of the Gaussian distribution. Each rotation appears twice (h and −h), but this is harmless: just use h as sampled.

python
import numpy as np

def sample_so3_uniform() -> np.ndarray:
    """Sample uniformly random rotation as unit quaternion [a,b,c,d].

    Haar measure on SO(3): invariant under rotations.
    Method: normalize 4 independent Gaussians.
    """
    h = np.random.randn(4)   # 4 independent N(0,1)
    h /= np.linalg.norm(h)    # project onto S³
    if h[0] < 0:
        h = -h                # canonical: a >= 0
    return h

def so3_geodesic_dist(h1: np.ndarray, h2: np.ndarray) -> float:
    """Geodesic distance between two SO(3) elements (as unit quaternions)."""
    dot = abs(np.dot(h1, h2))   # |h1·h2|, handles double-cover
    dot = np.clip(dot, 0.0, 1.0)
    return 2.0 * np.arccos(dot)  # angle in [0, pi]

def se3_distance(t1, h1, t2, h2, alpha=1.0, beta=1.0) -> float:
    """Weighted SE(3) configuration distance."""
    dt = np.linalg.norm(t1 - t2)
    dr = so3_geodesic_dist(h1, h2)
    return np.sqrt(alpha * dt**2 + beta * dr**2)

# Example: two SE(3) configurations
t1 = np.array([1.0, 0.0, 0.5])
h1 = axis_angle_to_quat(np.array([0, 0, 1]), 0)         # no rotation

t2 = np.array([2.0, 1.0, 0.5])
h2 = axis_angle_to_quat(np.array([0, 0, 1]), np.pi/2)   # 90° around Z

d = se3_distance(t1, h1, t2, h2, alpha=1.0, beta=1.0)
# d ≈ sqrt(1² + 1² + 0² + (π/2)²) ≈ sqrt(2 + 2.47) ≈ 2.11
# (1m translation in x, 1m in y, 90° rotation)

Summary: C-space topology reference table

Here is the complete reference for everything covered in Chapters 0–5 of this lesson. This table is the core “cheat sheet” for designing motion planners for different robot types:

Choosing a quaternion library: what to look for

When implementing SE(3) planning, you will use a quaternion library. Key features to verify: (1) Unit normalization: the library should normalize after every multiplication to prevent numerical drift off S³. (2) Double-cover handling: distance and interpolation functions should compare h with both h₂ and −h₂, using the shorter arc. (3) SLERP: the library should implement spherical linear interpolation correctly, including the special case when the two quaternions are nearly identical (use linear interpolation when |h₁·h₂| > 0.9995 to avoid division by sin(Ω) ≈ 0). (4) Consistent convention: the library should document whether quaternions are stored as (w, x, y, z) or (x, y, z, w) — a common source of bugs when mixing libraries.

Common Python options: scipy.spatial.transform.Rotation (the standard, wraps quaternions, supports SLERP, handles all edge cases), pyquaternion (lightweight, explicit), numpy-quaternion (fast, C extension). ROS uses its own convention (x, y, z, w in geometry_msgs/Quaternion). PyBullet uses (x, y, z, w). Eigen uses (w, x, y, z) internally. Always check the convention before integrating a library.

Chapter 6: Kinematic Chains

A robot arm is not a single rigid body. It is a kinematic chain: a sequence of rigid links connected by joints. Each joint contributes one or more degrees of freedom, and those DOF multiply together to form the full C-space. Understanding how the pieces combine — topologically, not just numerically — is the difference between a planner that works and one that misses valid paths.

The rule is simple and powerful. If joint 1 has C-space factor C₁ and joint 2 has factor C₂, then the two-joint arm has C = C₁ × C₂. Each joint contributes its own topological piece independently. The C-space of the whole chain is the Cartesian product of the pieces.

The canonical example: the 2-link planar arm

Two revolute joints, each with full 360° rotation. Joint 1 contributes S¹. Joint 2 contributes S¹. Therefore:

T² is the 2-torus: a donut shape. The C-space of this arm is not a flat square — it is a square with opposite edges identified, which is topologically a torus. Paths that cross the θ₁=0/2π boundary or the θ₂=0/2π boundary are perfectly valid; they wrap around the torus.

Forward kinematics: from angles to end-effector

Given joint angles (θ₁, θ₂), the forward kinematics maps them to the end-effector position in the workspace. For a planar 2-link arm with link lengths L₁ and L₂:

The elbow joint position is (L₁cosθ₁, L₁sinθ₁). The hand tip is that plus the second link rotated by the total angle θ₁+θ₂. One point in C-space (a pair of angles) maps to one point in the workspace. But one point in workspace can have two preimages in C-space — elbow-up and elbow-down. This is why planning in C-space is more natural: there is no ambiguity.

Showcase: workspace and C-space side by side

The canvas below shows both views simultaneously. LEFT: the physical arm in 2D workspace with a circular obstacle. RIGHT: C-space as a square (toroidal identification shown by arrows on the edges). The orange dot is the current configuration (θ₁, θ₂). The dark region in the right panel is C_obs — precomputed on a 60×60 grid by checking collision at each (θ1,θ2). Click anywhere in the right (C-space) panel to jump to that configuration and watch the arm move. Notice how the dot can exit one edge and appear on the opposite edge — the torus in action.

python
import numpy as np

def fk_2link(theta1: float, theta2: float,
              L1: float = 1.0, L2: float = 0.8) -> tuple:
    """Forward kinematics for a planar 2-link arm.
    Returns (elbow_x, elbow_y, hand_x, hand_y).
    """
    ex = L1 * np.cos(theta1)
    ey = L1 * np.sin(theta1)
    hx = ex + L2 * np.cos(theta1 + theta2)
    hy = ey + L2 * np.sin(theta1 + theta2)
    return ex, ey, hx, hy

def seg_dist(p, a, b) -> float:
    """Distance from point p to segment [a, b]."""
    ab = b - a
    t = np.clip(np.dot(p - a, ab) / (np.dot(ab, ab) + 1e-12), 0, 1)
    return np.linalg.norm(p - (a + t * ab))

def arm_collides(t1, t2, obs_c, obs_r, L1=1.0, L2=0.8) -> bool:
    ex, ey, hx, hy = fk_2link(t1, t2, L1, L2)
    orig  = np.array([0.0, 0.0])
    elbow = np.array([ex, ey])
    hand  = np.array([hx, hy])
    obs   = np.array(obs_c)
    return (seg_dist(obs, orig, elbow) < obs_r or
            seg_dist(obs, elbow, hand) < obs_r)

# Precompute C_obs on a 60×60 grid over [0, 2π) × [0, 2π)
N = 60
ts = np.linspace(0, 2*np.pi, N, endpoint=False)
cobs = np.array([[arm_collides(t1, t2, [1.2, 0.4], 0.35)
                  for t2 in ts] for t1 in ts])
# cobs.shape = (60, 60); True = collision at (ts[i], ts[j])

Chapter 7: C-Space Obstacles — Where Geometry Meets Topology

You now know what C-space is. The next question is: which configurations are forbidden? The answer is the C-space obstacle region, C_obs. This is the set of all configurations where the robot touches or overlaps at least one obstacle in the workspace.

Here A(q) is the set of workspace points occupied by the robot at configuration q. O is the union of all workspace obstacles. C_free = C ∖ C_obs is the open set where motion is legal. The planning problem is: find a continuous path through C_free.

Multi-link robots: the full collision formula

For a kinematic chain with links A₁, …, A_m, collisions come from two sources: each link might hit a workspace obstacle, and non-adjacent links might hit each other. Let P be the set of collision pairs — pairs [i, j] of links that are not permanently attached and could potentially intersect:

Adjacent links share a joint and are always in contact at the joint point; they are NOT in P. Self-collision only becomes significant for 3+ link chains. For the 2-link arm, collision pairs are simply (link1, obstacle) and (link2, obstacle).

The Piano Mover’s Problem (Formulation 4.1)

LaValle’s Formulation 4.1 is the canonical statement of the motion planning problem in C-space:

The “correctly report” clause is critical. A planner that always says “no path found” trivially satisfies the first condition but fails the second. This is the completeness requirement. The Piano Mover’s Problem is PSPACE-hard (Reif, 1979): as DOF grows, worst-case complexity explodes exponentially. No polynomial-time algorithm for the general case exists, which motivates sampling-based approximations.

Interactive: translating robot, C-space, C_obs

Chapter 8: Minkowski Sum — Computing C_obs Exactly

For a robot that only translates (no rotation), there is an elegant closed-form formula for C_obs. It uses the Minkowski sum. Master this in 1D first, then 2D.

The definition

Geometrically: place a copy of A at every point in B (or equivalently, place a copy of B at every point in A) and union all the translates. The result is the set of all points reachable by adding one element from each set.

1D worked example (LaValle Example 4.13)

Robot A = [−1, 2] (occupies 3 units; reference point at 0, extends left 1 unit and right 2 units). Obstacle O = [0, 4]. We need C_obs = O ⊕ (−A).

Step 1: negate the robot. −A = [−2, 1]. Step 2: Minkowski sum. O ⊕ (−A) = {o + r | o ∈ [0,4], r ∈ [−2,1]} = [0−2, 4+1] = [−2, 5].

2D convex polygons (LaValle Example 4.14)

For convex polygons in 2D, the Minkowski sum boundary is computed by merging the edge sequences of O and −A, sorted by the angle of their outward normals. If O has m edges and A has n edges, C_obs = O ⊕ (−A) has at most m+n edges and can be computed in O(m+n) time (after sorting).

Two types of contact contribute to the boundary: EV contact (an edge of A touches a vertex of O) produces an edge from A in the boundary. VE contact (a vertex of A touches an edge of O) produces an edge from O in the boundary. The merge procedure walks both polygons simultaneously, always advancing along the edge with the smaller outward normal angle.

Interactive: robot size → C_obs size

python
import numpy as np

def minkowski_convex(P: np.ndarray, Q: np.ndarray) -> np.ndarray:
    """Minkowski sum of two convex CCW polygons.

    Merge edge sequences sorted by outward-normal angle.
    O(n+m) once polygons are bottom-aligned.
    """
    # Start at bottom-most vertex of each polygon
    i0 = int(np.argmin(P[:, 1]))
    j0 = int(np.argmin(Q[:, 1]))
    n, m = len(P), len(Q)
    result, i, j = [], i0, j0

    for _ in range(n + m):
        result.append(P[i % n] + Q[j % m])
        ep = P[(i+1)%n] - P[i%n]   # current edge of P
        eq = Q[(j+1)%m] - Q[j%m]   # current edge of Q
        cross = ep[0]*eq[1] - ep[1]*eq[0]
        if   cross > 0: i += 1      # advance P
        elif cross < 0: j += 1      # advance Q
        else:            i += 1; j += 1  # parallel edges

    return np.array(result)

# Square robot half-side s, square obstacle half-side w:
# C_obs is a square with half-side (s + w)
# — Minkowski sum of two squares = larger square with summed side lengths
s, w = 0.2, 0.5
# C_obs side length = 2*(s+w) = 2*(0.2+0.5) = 1.4

Chapter 9: Why Explicit C_obs Breaks Down at Scale

The Minkowski sum is elegant for translation-only robots. But real robots rotate, and real robots have many joints. The moment you add rotation, explicit C_obs computation becomes intractable. This is not an engineering inconvenience — it is a fundamental barrier, and it is WHY sampling-based methods exist.

Adding rotation: C_obs becomes a 3D surface

For a robot that translates AND rotates in 2D, C = R²×S¹. At each fixed angle θ, the 2D cross-section of C_obs is O ⊕ (−A_θ), where A_θ is the robot rotated by θ. Because −A_θ changes shape as θ varies, the Minkowski sum is different at every angle. C_obs is a 3D region in (x, y, θ)-space with a continuously morphing cross-section that cannot be computed efficiently for non-convex shapes.

High-DOF chains: C_obs in Tⁿ

For an n-DOF revolute arm, C_obs lives in Tⁿ. For n=6, that is a subset of a 6-dimensional torus. You cannot visualize it, store it, or compute its boundary. Even a coarse grid with 10 values per dimension needs 10⁶ cells. At 100 per dimension: 10¹² cells, each requiring a collision check. Completely infeasible.

The 2D cross-section view: rotation changes everything

The canvas shows a triangular robot rotating in front of a rectangular obstacle. The left panel shows the robot at the current angle θ. The right panel shows the 2D cross-section of C_obs at that θ — the Minkowski sum O ⊕ (−A_θ) computed on a grid. Slide θ and watch the C_obs cross-section morph. This is why you cannot just store C_obs once — it is different for every θ.

Chapter 10: Showcase — The C-Space Pipeline

Every motion planning system ever built for a real robot follows the same pipeline. You define the robot’s geometry, identify the C-space topology, build a collision checker, hand everything to a planner, and get back a path. The C-space abstraction is the seam that separates geometry from search — and that separation is what makes the whole thing work at scale.

The pipeline from geometry to execution

Chapter 11: Connections — Manifolds, Maps, and What Comes Next

You’ve built the full C-space vocabulary across 12 chapters. Here is the complete reference — every manifold, every distance formula, every topology — in one place. Use this as your cheat sheet when designing motion planners for new robot types.

Complete C-space reference table

Key formulas

Limitations and what comes next

High dimensionality. A 7-DOF arm lives in a 7D manifold. Sampling-based planners work, but narrow passages in C_free become exponentially harder to find as dimension grows. This is the active research frontier: narrow-passage sampling, bridge testing, workspace guidance.

Kinodynamic constraints. The C-space framework assumes the robot can stop anywhere and move in any direction. Real robots have velocity limits and momentum. Kinodynamic planning extends C-space to state space (q, q˙) and is covered in LaValle Chapter 14.

Uncertainty. Perfect configuration knowledge is assumed here. In practice, sensors are noisy and maps are imperfect. Belief-space planning (POMDPs) replaces the configuration with a probability distribution and is covered in Chapter 11.

Chapter connections