Configuration Space II: Topology, Manifolds, and Rigid Bodies

What "the edges are identified" means, why every planner needs a distance, an interpolation and a sampler that respect it, and the Manifold trait that makes the compiler enforce the difference between a torus and the plane.

The problem: a path in two pieces

Here is the Chapter 4 picture of Reach's configuration space again — the square $[-\pi, \pi]^2$ with the C-obstacles of three disc obstacles on the Workbench — and a purple path between two configurations. On the square the path is two disconnected pieces. Watch the square fold.

When the square has become a doughnut the two pieces are one curve, and a short one. Nothing about the configurations changed; the picture of the space changed, from a chart to the space. The readout underneath says the same thing in numbers. For the default endpoints the chart's Euclidean distance is about six, the torus distance well under one, and the difference is entirely the seam: the path crosses the dashed edge where $\theta_1 = \pi$ becomes $\theta_1 = -\pi$.

Three things in the widget are the whole chapter.

The seam is an artifact of the chart, not a feature of the space. The square is one way of assigning two numbers to a configuration of a two-joint arm. It is a good way almost everywhere and a bad way along four edges, and a planner that works with the numbers inherits the bad edges. Choset's §3.4.2 names what the square is — a chart — and what the torus needs instead — an atlas.

Distance belongs to the space. dist in the readout is $d_{T^2}$, the metric this chapter defines on the torus. It is shorter than the chart distance exactly when the short way crosses a seam. Chapter 11's nearest-neighbor queries, Chapter 12's tree extensions and Chapter 13's rewiring radii all call this function, and all of them would quietly plan around a wall that does not exist if they called the chart's distance instead.

Joint limits change the topology. Switch them on. The square stops wrapping, the surface on the right is a disc, and the component count in the readout changes: the band of C-obstacle that cut the square into two components, which the torus had glued back into one, now separates them for good. Choset's §3.4.3 says this in one line — compact and noncompact spaces cannot be diffeomorphic — and the lab at the end of the chapter measures it.

Building intuition

An angle is not a number

Start with the simplest manifold that is not a Euclidean space: the circle $S^1$, the configuration space of one revolute joint. Choset's §3.4.2 covers it with four charts — the open half-circles $U_1 = \{x_2 > 0\}$, $U_2 = \{x_2 < 0\}$, $U_3 = \{x_1 > 0\}$, $U_4 = \{x_1 < 0\}$, each coordinatized by whichever Cartesian coordinate varies monotonically on it. The widget hands a point from chart to chart.

Every point of the circle lies in at least one chart — the four domains cover. Wherever a point lies in two, both coordinates exist and are related by a smooth function, the transition map; on $U_1 \cap U_3$ it is $\varphi_1 \circ \varphi_3^{-1}(z) = \sqrt{1 - z^2}$, drawn at lower right. That smoothness is what lets a planner, or a derivative, pass from one chart to the next without a jolt. The two-chart angle atlas is the one S1 uses in code: the angle measured from $+x$ on the circle minus $(-1, 0)$, and the angle measured from $-x$ on the circle minus $(1, 0)$, whose transition map is a translation by $\pi$.

Then try the single-chart attempt, $\theta \in [0, 2\pi)$. It assigns a number to every point, and it is not a chart: drag through $\theta = 0$ and the coordinate tears from $6.28$ to $0$. A chart's domain has to be open, so that every point has a neighborhood on which the map is continuous, and no neighborhood of $(1, 0)$ maps continuously to an interval. Chapter 4's "single angle in $[0, 2\pi)$" was a chart with a seam, not an atlas, and the seam is where its arithmetic lies.

A transform is not a position plus an angle

The second intuition is about rigid bodies. Choset's §3.5.1 works one example completely — three frames $A$, $B$, $C$ on a unit grid and a point $w$ — and it is the one place in his book where the group $\SEtwo$ can be checked by hand. The widget keeps his numbers as its reset state.

Two rules generate everything in the readout. Subscript cancellation: $T_{AB}\,T_{BC} = T_{AC}$, and $T_{BC}\,w_C = w_B$ — the inner subscripts annihilate, and the result is the outer pair. And non-commutativity: the same transform $T_1$ multiplied on the right of $T_{AB}$ moves frame $B$ in its own coordinates (a body-frame transformation, $B'$), multiplied on the left it moves $B$ in $A$'s coordinates (a world-frame transformation, $B''$), and the two frames land in different places. Autoplay sweeps $T_1$'s heading so you can watch $B'$ and $B''$ diverge; reset returns to Choset's $T_1$ and the two placements $(-2, -1, 0)$ and $(2, 1, 0)$. A pose is an element of a group. The order of multiplication is the whole content.

The third intuition — that gimbal lock is a missing chart — waits for the rotation section, where it has its own widget.

The mathematics

Mappings: onto, one-to-one, and the two kinds of "the same"

Let $\phi : S \to T$ be a mapping. It is surjective (onto) if $\phi(S) = T$, injective (one-to-one) if $\phi(s_1) = \phi(s_2)$ forces $s_1 = s_2$, and bijective if both. A bijection has an inverse everywhere on $T$, and this is what lets us move back and forth between a configuration space, whose geometry may be complicated, and a Euclidean space, whose geometry is not. Choset's example: $\sin : (-\pi/2, \pi/2) \to (-1, 1)$ is bijective; $\sin : \mathbb{R} \to [-1, 1]$ is only surjective.

The converse fails on Choset's three curves (his figure 3.12): the unit circle $M_c = \{x^2 + y^2 = 1\}$, the ellipse $M_e = \{x^2/4 + y^2 = 1\}$, and the racetrack $M_r$ — two half-circles of radius one joined by straight segments, $f_r(x,y) = 0$ with

All three are homeomorphic: radial projection $\phi(x, y) = (x, y)/\sqrt{x^2 + y^2}$ carries the ellipse onto the circle and is continuous both ways. The circle and ellipse are diffeomorphic — that same $\phi$ is smooth both ways. But no diffeomorphism reaches the racetrack: its curvature jumps at $(\pm 1, \pm 1)$, where straight meets round, and a diffeomorphism would have to carry the circle's constant curvature onto it smoothly. Topology sees one closed curve; differential structure sees two kinds.

Neighborhoods, and what "locally" means

A neighborhood of a point $p$ in a metric space is any set $U \ni p$ such that every point of $U$ has an open ball $B_\epsilon(p') = \{p'' : d(p', p'') < \epsilon\}$ inside $U$; an open ball is itself a neighborhood. $S$ is locally diffeomorphic to $T$ if every point of $S$ has a neighborhood diffeomorphic to an open subset of $T$. The sphere is locally diffeomorphic to the plane — which is Choset's remark about the flat earth in this chapter's epigraph — and not globally: there is no bijection from the sphere onto the plane that is continuous both ways.

The two robots of Chapter 4 sit on either side of this distinction. A disc translating in the plane has configuration space $\mathbb{R}^2$, globally diffeomorphic to its workspace by the identity. The two-joint arm has $T^2$, which is locally diffeomorphic to $\mathbb{R}^2$ — every configuration has a neighborhood that looks like a patch of the plane — and not globally, because $T^2$ is compact and $\mathbb{R}^2$ is not (§3.4.3 below). Give the arm strict joint limits $\theta_i^\ell < \theta_i < \theta_i^u$ and its configuration space becomes an open rectangle, which is globally diffeomorphic to $\mathbb{R}^2$: stretch each open interval over the line with $\tan$. Joint limits are not a detail. They change the topology.

Manifolds, charts, atlases

The configuration spaces this book plans in are all differentiable manifolds, and the parameterizations of Chapter 4 — "the configuration of the arm is $(\theta_1, \theta_2)$" — were charts without the name. When no single chart covers the space, as for the torus, there are three options (Choset §3.5): use one chart anyway and suffer its seam; use an atlas; or embed the space in a higher-dimensional Euclidean space with constraints — the unit circle in $\mathbb{R}^2$, the doughnut in $\mathbb{R}^3$, rotation matrices in $\mathbb{R}^9$. The code in this chapter uses all three, each where it is best.

Connectedness and compactness

A manifold is connected if any two of its points are joined by a path. $\mathbb{R}^n$, $S^n$, $T^n$ are; so are $\SOthree$ and $\SEtwo$. Obstacles break $\Qfree$ into connected components, its maximal connected subsets, and the first fact every planner must respect is that there is no solution when $\qstart$ and $\qgoal$ lie in different components. The Torus Unwrapper's component count is this definition evaluated on a raster.

A space is compact if it is a closed and bounded subset of some $\mathbb{R}^n$. $[0, 1]$ is compact and $[0, 1)$ is not; $S^n$, $T^n$ and $\SOthree$ are compact; $\mathbb{R}^n$ and $\SEtwo$ are not. Products of compact spaces are compact, and a non-compact product $M_1 \times M_2$ with $M_1$ compact has $M_1$ as its compact factor — $\SEtwo \cong \mathbb{R}^2 \times S^1$ has $S^1$. Two facts matter for planning: compact and non-compact spaces are never diffeomorphic, which is the invariant that separates $T^2$ from $\mathbb{R}^2$; and a sampler can be uniform on a compact space but needs a box on a non-compact one, which is why Se2::sample takes bounds and S1::sample does not.

Metric spaces, and the contract for dist

Non-negativity is not an axiom; it follows: $0 = d(m_1, m_1) \le d(m_1, m_2) + d(m_2, m_1) = 2 d(m_1, m_2)$. A function $f : S \to T$ between metric spaces is continuous (Definition C.4.1) when preimages of open sets are open, equivalently when for every $\epsilon > 0$ there is a $\delta > 0$ with $d(x, s) < \delta \Rightarrow d(f(x), f(s)) < \epsilon$. A path is a $C^0$ map $c : [0, 1] \to \Qfree$; a trajectory must be $C^k$ for $k \ge 1$ so that velocity exists.

Here are the four metrics this chapter ships, with the chart's wrong answer written beside each right one. The seam example from the hook: angles $a = 0.1$ and $b = 6.2$ radians.

The geodesic midpoint interpolate(a, b, 0.5) goes the short way, downward from $0.1$ by half of $0.1832$: it lands at $\htmlClass{term-path}{0.0084}$ after wrapping, not at the chart midpoint $3.15$. On the torus, with $p = (0.1, 3.0)$ and $q = (6.2, 3.5)$:

On $\SEtwo$ the metric needs an exchange rate $w$ between metres and radians, and with $w = 0.5\ \mathrm{m/rad}$ the distance from $(0, 0, 0)$ to $(1, 0, \pi)$ is

On $\SOthree$ the distance between two rotations is the angle of the rotation that carries one to the other, $d_{SO(3)}(R, R') = \arccos\big((\operatorname{tr}(R^{\mathsf T} R') - 1)/2\big)$, which is also the axis–angle $\theta$ of $R^{\mathsf T} R'$ (App. E.3) and, for unit quaternions, $2\arccos|\langle Q, Q' \rangle|$.

Embeddings: $SO(n)$ and $SE(n)$

The alternative to an atlas is an embedding with constraints. A rotation in $\mathbb{R}^3$ is a $3 \times 3$ matrix whose columns are the body axes $\tilde x, \tilde y, \tilde z$ expressed in a stationary frame: nine numbers, six constraints (three unit lengths, three orthogonalities), three degrees of freedom.

A matrix in $SE(n)$ is used three ways (§3.5.1): to represent a configuration — then it is a frame; to change the reference frame of a configuration or a point; and to displace one — then it is a transform. The three frames of the Frame Composer are Choset's figure 3.17 restricted to the plane, with $T_{AB}$ having $R = \mathrm{rot}(\pi)$, $p = (-2, 0)$ and $T_{BC}$ having $R = \mathrm{rot}(-\pi/2)$, $p = (-4, -1)$.

$D\varphi$ is chart-independent; the Jacobian is its matrix

Chapter 4 computed Reach's Jacobian $J(q) = \partial\varphi/\partial q$ for the forward map $\varphi : T^2 \to \mathbb{R}^2$. Appendix C.5 calls the same matrix the differential $D\varphi$ and adds the one footnote that matters here: the differential is really a linear map from the tangent space of the domain to the tangent space of the range, $D\varphi_q : T_q\Q \to T_{\varphi(q)}M$, and the matrix is its expression in charts. Along a curve $c(t)$ in $\Q$, $\tfrac{d}{dt}\varphi(c(t)) = D\varphi_{c(t)}\,\dot c(t)$ — velocities are columns, pushed forward by $J$. Forces live in the cotangent space and are rows, pulled back by $J^{\mathsf T}$ (Choset's remark on p. 486), which is why Chapter 7 will write $\tau = J^{\mathsf T} F$.

Rotations in three dimensions, briefly

Everything above generalizes from $SO(2)$ to $\SOthree$ except the coordinates. Nine matrix entries with six constraints leave three degrees of freedom, so $\SOthree$ is a three-dimensional manifold and can be locally parameterized by three numbers — Euler angles — but, just as for the circle, not globally.

Choset's choice is Z-Y-Z Euler angles $(\phi, \theta, \psi)$ (§3.6): rotate about $z$ by $\phi$, then about the new $y$ by $\theta$, then about the new $z$ by $\psi$, so that

One more honesty item about $\SOthree$, because Chapter 11's samplers depend on it. Uniform on $\SOthree$ means uniform with respect to the Haar measure, the one invariant under rotation. A normalized Gaussian 4-vector is uniform on the quaternion 3-sphere and therefore Haar-uniform on $\SOthree$. Uniform Euler angles are not: the rotation angle of a Haar-uniform rotation has density $(1 - \cos\theta)/\pi$ on $[0, \pi]$, mean $\pi/2 + 2/\pi = 2.2074$, and only $(0.5 - \sin 0.5)/\pi = 0.65\%$ of rotations lie within half a radian of the identity; drawing $(\phi, \theta, \psi)$ uniformly puts about three times that mass near the identity. So3::sample is the Gaussian version, and a check pins both numbers.

The algorithm

This chapter's algorithms are operations, not planners: the three methods a manifold must provide, and the first generic planner-shaped function built on them.

The per-manifold operations, in the order the Rust implements them:

Line 2 of the $\SOthree$ box is a numerical honesty item worth a sentence: the first implementation of dist used $2\arccos d$ directly, and dist(Q, Q) came out as $6 \times 10^{-8}$, which failed the definiteness check. The half-angle identity $\arccos x = 2\operatorname{atan2}(\sqrt{1-x}, \sqrt{1+x})$ gives the same number to full precision, and a metric that is not definite to machine precision breaks every equality test built on it.

Implementation in Rust

The trait is the chapter. Everything else is four implementations of it, a product, and one generic function.

use prob::Rng;

/// THE OWNED ARTIFACT. Every planner from Chapter 6 on is generic over this trait
/// and never touches coordinates directly. `DIM` is the manifold dimension — the
/// number of local coordinates — not the embedding dimension (3 for SO(3), not 9).
pub trait Manifold: Copy + PartialEq + core::fmt::Debug {
    const DIM: usize;
    /// Sampling box for non-compact factors; `()` for compact spaces, which have
    /// a uniform distribution of their own and need no box.
    type Bounds;

    /// A metric in the sense of Definition C.2.1 — property-tested, not assumed.
    fn dist(&self, other: &Self) -> f64;
    /// The geodesic from `self` to `other` at `t ∈ [0, 1]`, taken the short way.
    fn interpolate(&self, other: &Self, t: f64) -> Self;
    /// Uniform with respect to the natural measure: Lebesgue on boxes, Haar on groups.
    fn sample(rng: &mut Rng, bounds: &Self::Bounds) -> Self;
}

/// The first generic planner-shaped function in the book: a geodesic local path.
/// Chapter 11's `trait Steer` defaults to exactly this.
pub fn geodesic_path<M: Manifold>(a: &M, b: &M, n: usize) -> Vec<M> {
    (0..=n).map(|i| a.interpolate(b, i as f64 / n as f64)).collect()
}