Configuration Space I: Obstacles in the Space of Configurations

Statement. For a disc of radius $r$ translating in the plane, with configuration its center $q$, $\QO_i = \WO_i \oplus B_r(0) = \{o + b : o \in \WO_i,\ \|b\| \le r\}$. For a convex polygon $\WO$ with perimeter $p$ the inflated area is $|\WO| + r\,p + \pi r^2$.

Step 1 — the preimage, unpacked. $R(q) = q + B_r(0)$. Then $R(q) \cap \WO \ne \emptyset$ iff there is an $o \in \WO$ with $\|q - o\| \le r$ iff $q = o + b$ for some $o \in \WO$, $\|b\| \le r$, iff $q \in \WO \oplus B_r$. Nothing about the disc's shape entered except that it is a ball about its reference point: the sum is exactly the obstacle "grown" by $r$.

Step 2 — the boundary. The boundary of $\WO \oplus B_r$ for a convex polygon consists of each edge offset outward by $r$ along its normal, joined at each vertex by a circular arc of radius $r$ sweeping from the incoming edge's normal to the outgoing edge's normal. inflateDisc builds exactly this: arcSegments chords per full turn, so the chord polygon approaches the true inflation from the inside.

Step 3 — the area. The offset strips contribute $r$ times the perimeter $p$. The arcs at the vertices have angles that sum to the total turning of a convex polygon, $2\pi$, so together they make one full disc, $\pi r^2$. For $\WO = [1, 2]^2$ and $r = 0.5$: $1 + 0.5 \cdot 4 + \pi \cdot 0.25 = \mathbf{3.7854}$. The check inflate asserts the closed form and that the chord polygon's area climbs toward it from below — $3.7804$ with $32$ chords, $3.7854$ with $720$. $\blacksquare$

Choset's warning (his page 44): "although both the workspace and the configuration space for this system can be represented by $\mathbb{R}^2$, and the obstacles appear to simply 'grow' in this example, the configuration space and workspace are different spaces, and the transformation from workspace obstacles to configuration space obstacles is not always so simple." A rotating body's footprint is not a ball about any point; Derivations 3 and 4 are what "not so simple" costs.

Statement. A planar rigid body has three degrees of freedom and a spatial one six; a system of $n$ coordinates with $m$ independent holonomic constraints has $n - m$.

Step 1 — place $A$ freely. Fix three non-collinear points $A, B, C$ on the body. $A$'s position is free: $2$ coordinates in the plane, $3$ in space.

Step 2 — $B$ is constrained to a circle (sphere). Rigidity fixes $d(A, B)$, so $B$ lies on a circle about $A$ in the plane — one angle $\theta$ remains — or on a sphere in space, where two angles remain.

Step 3 — $C$ follows, up to a discrete choice. Two fixed distances $d(A, C)$, $d(B, C)$ place $C$ in the plane up to a reflection: zero continuous freedoms remain. In space, three distances leave $C$ on a circle about the axis $AB$: one angle remains. Every other point of the body is then fixed by its three distances. Totals: $2 + 1 = 3$ and $3 + 2 + 1 = 6$, with configuration spaces $\mathbb{R}^2 \times S^1$ and $\mathbb{R}^3 \times SO(3)$. $\blacksquare$

Reach, counted the indirect way (Choset's own example). Treat each link as a spatial rigid body: $12$ coordinates. Six constraints confine the two links to the table plane, two pin the first joint to a point, and — once link 1's angle is chosen — two pin the second joint to the end of link 1: $12 - 10 = 2$. The direct count is shorter: one revolute joint, one degree of freedom, twice. A closed chain: Choset's figure 3.9 has six links and seven revolute joints in the plane, so $M = 3(6 - 7 - 1) + 7 = 1$ — a one-parameter mechanism, like the four-bar linkage of problem 3.7.

Statement. For fixed $\theta$, $\QO(\theta) = \{(x, y) : (x, y, \theta) \in \QO\}$ is a convex polygon, and $q \in \QO(\theta)$ iff every applicable contact's half-plane inequality holds. A Type A pair $(E^R_i, o_j)$ is applicable (eq. F.3) when

equivalently when the negated robot normal $-n^R_i$ lies between the normals of the two obstacle edges meeting at $o_j$; its half-plane is (eq. F.4) $f^R_{ij}(q) = n^R_i(\theta) \cdot (o_j - r_i(x, y, \theta)) \le 0$. Symmetrically a Type B pair $(E^W_j, r_i)$ is applicable (F.5) when $(r_{i-1} - r_i) \cdot n^W_j \ge 0$ and $(r_{i+1} - r_i) \cdot n^W_j \ge 0$, with half-plane (F.6) $f^W_{ij}(q) = n^W_j \cdot (r_i(x, y, \theta) - o_j) \le 0$.

Step 1 — the boundary is where the bodies touch. If the interiors overlap at $q$, every nearby $q'$ is also in $\QO(\theta)$, so $q$ is interior to $\QO(\theta)$. Boundary configurations are therefore exactly those satisfying F.2: contact without overlap.

Step 2 — only Type A and Type B realize contact. Two convex polygons whose interiors are disjoint meet in a point or a segment of their boundaries; the meeting set contains a vertex of one body lying on an edge of the other (or an edge along an edge, which is both). That is the dichotomy.

Step 3 — each applicable pair gives a supporting half-plane. Take an applicable Type A pair. The condition F.3 says both obstacle edges leaving $o_j$ point into or along the robot normal's half-space, so the whole obstacle lies on the side $n^R_i \cdot (o - r_i) \ge n^R_i \cdot (o_j - r_i)$; the robot edge $E^R_i$ can touch the obstacle only at $o_j$. Translating the robot, the contact persists while $n^R_i \cdot (o_j - r_i(q)) = 0$ — a line in the $(x, y)$-plane — and the robot overlaps the obstacle exactly on the side $f^R_{ij} \le 0$. So $\QO(\theta) \subseteq \{f^R_{ij} \le 0\}$ and the line is a supporting line of $\QO(\theta)$. Type B is the same with roles swapped. Non-applicable pairs give nothing: a robot edge whose normal does not point at $o_j$ cannot touch it from outside.

Step 4 — a convex set is the intersection of its supporting half-planes. $\QO(\theta)$ is convex (it is a Minkowski difference of convex sets, Derivation 4), and every edge of its boundary is a contact line from Step 3, so $\QO(\theta)$ equals the intersection of the applicable half-planes. Hence $q \in \QO(\theta)$ iff all applicable inequalities hold — and one failing inequality is a certificate of separation. $\blacksquare$

Why $n^R_i$ depends on $\theta$ only. The robot's normals rotate with it and do not move with it; applicability is a function of $\theta$ alone, and the star algorithm exploits this by sorting normals once per heading.

Nonconvex bodies. Decompose both into convex pieces $\{R_l\}$, $\{W_k\}$ and test every pair; the union of the pairwise $\QO$'s is the C-obstacle (the union property above).

Statement. Negate the robot's edge normals; merge-sort them with the obstacle's by angle; scan the merged list once. Each time a negated robot normal $-n^R_i$ falls between adjacent obstacle normals, a Type A contact of $E^R_i$ with the vertex $o_j$ between those normals is applicable, and it contributes the vertices $o_j - r_i$ and $o_j - r_{i+1}$; each time an obstacle normal $n^W_j$ falls between adjacent negated robot normals, a Type B contact of $E^W_j$ with $r_i$ is applicable and contributes $o_j - r_i$ and $o_{j+1} - r_i$. The emitted points, in scan order, are the vertices of $\QO(\theta)$ counterclockwise, and $\QO(\theta)$ equals the Minkowski difference (eq. F.7) $\WO \ominus R = \{o - r : o \in \WO,\ r \in R\}$.

Step 1 — contacts persist along edges, and their extremes are vertices. Fix an applicable Type A pair. Slide the robot so that $o_j$ stays on edge $E^R_i$: the configuration traces a segment of the boundary of $\QO(\theta)$. At one extreme $o_j$ coincides with $r_i$, at the other with $r_{i+1}$; the reference point is then at $o_j - r_i(0, 0, \theta)$ and $o_j - r_{i+1}(0, 0, \theta)$. These are two vertices of $\QO(\theta)$, and the edge between them is a translate of $E^R_i$ — with outward normal $-n^R_i$, since the robot is reflected through its reference point. Symmetrically a Type B pair gives the vertices $o_j - r_i$ and $o_{j+1} - r_i$, joined by a translate of $E^W_j$ with normal $n^W_j$.

Step 2 — angular order of normals is boundary order. The boundary of a convex polygon, walked counterclockwise, has outward normals that rotate monotonically through $2\pi$. The boundary of $\QO(\theta)$ is made of translates of robot edges (normals $-n^R_i$) and obstacle edges (normals $n^W_j$), so walking it counterclockwise visits those edges in the angular order of $\{-n^R_i\} \cup \{n^W_j\}$. Merging the two already-sorted lists is that walk.

Step 3 — the merged scan visits every applicable pair exactly once. Keep a current robot vertex $i$ and obstacle vertex $j$, starting from the pair extreme in the $+x$ direction. Consuming a robot entry means walking the (reflected) robot edge $E^R_i$ while $o_j$ stays put — a Type A contact — and advancing $i$; consuming an obstacle entry means walking $E^W_j$ while $r_i$ stays put — Type B — and advancing $j$. Each entry is consumed once, so each applicable contact is emitted once and the scan closes on its start after $n_R + n_W$ steps. Cost: $O((n_R + n_W)\log(n_R + n_W))$ for a general sort, $O(n_R + n_W)$ here because each list is already in order.

Step 4 — equality with the hull of all pairwise differences. $\WO \ominus R = \WO \oplus (-R)$ is a Minkowski sum of convex polygons, hence convex, and its extreme points are differences of extreme points, so it is the convex hull of the $n_R n_W$ points $o_j - r_i$. The star algorithm's vertices are among these points and its edges have the correct normals, so the two polygons coincide. That hull is minkowskiBruteForce, the oracle the check star holds the scan to on $500$ seeded convex pairs at random headings: identical polygons, worst area gap $2 \times 10^{-15}$. $\blacksquare$

The preset, worked. Robot $(0, 0), (0.2, 0), (0, 0.2)$ at $\theta = 0$; obstacle $[1, 2]^2$. The merged list is $-n^R_3$ at $0°$, $n^W_1$ at $0°$, $-n^R_1$ at $90°$, $n^W_2$ at $90°$, $n^W_3$ at $180°$, $-n^R_2$ at $225°$, $n^W_4$ at $270°$ — seven entries, seven steps. The emitted points are $(2, 0.8), (2, 1), (2, 2), (1.8, 2), (0.8, 2), (0.8, 1), (1, 0.8)$; two of them, $(2, 1)$ and $(1.8, 2)$, lie on edges because a robot edge is parallel to an obstacle edge, and dropping collinear vertices leaves the five the design doc names — $(1, 0.8), (2, 0.8), (2, 2), (0.8, 2), (0.8, 1)$ — with area $1 + 0.2 + 0.2 + 0.02 = \mathbf{1.42}$: the square, two strips, and the reflected triangle in the corner. Turn the robot to $\theta = 30°$ and no edges are parallel: seven vertices. The check star pins all of it.

Statement. $d(A, B) = \min_{a \in A, b \in B} \|a - b\| = \min_{z \in Z} \|z\|$ with $Z = A \ominus B$ (eqs. F.8–F.10); the minimizer $z^*$ is unique; and Gilbert, Johnson and Keerthi's iteration finds it using only the support value $h_Z(x) = \max_{z \in Z} z \cdot x$ and support point $s_Z(x)$ (F.11–F.12), which never require $Z$ to be built.

Step 1 — rewrite over the Minkowski difference. Every $a - b$ is a point of $Z$ and every point of $Z$ is some $a - b$, so the two minima are the same number (F.10). The distance between two bodies is the distance from one convex set to the origin.

Step 2 — uniqueness. $Z$ is convex (Derivation 4, Step 4) and $\|\cdot\|$ is a strictly convex function away from the origin, so $\arg\min_{z \in Z} \|z\|$ is a single point $z^*$. The witness pair $(a^*, b^*)$ with $a^* - b^* = z^*$ need not be unique — two parallel edges facing each other have a whole segment of nearest pairs — and Choset says so after F.10. Uniqueness is of the difference.

Step 3 — the simplex iteration. Keep a working set $V_k$ of at most three points of $Z$ (a point, a segment or a triangle in the plane) and let $x_k$ be the point of $\operatorname{hull}(V_k)$ nearest the origin. Ask $Z$ for its support point in direction $-x_k$: $z_k = s_Z(-x_k)$. If $h_Z(-x_k) = -\|x_k\|^2$ — no point of $Z$ projects farther toward the origin than $x_k$ itself — then $x_k = z^*$ and the iteration stops (Algorithm 23, step 4). Otherwise $z_k$ is strictly closer to the origin in the direction $-x_k$, so $\operatorname{hull}(V_k \cup \{z_k\})$ contains points nearer than $x_k$; keep the edge of the old simplex that contained $x_k$ plus $z_k$ (step 6) and repeat. The distance to the origin strictly decreases, the working set never revisits a vertex of $Z$ in exact arithmetic, and $Z$ has at most $n_A + n_B$ vertices, so the loop terminates in at most that many steps. If the origin lands inside the triangle, $d = 0$ and the bodies intersect.

Step 4 — support functions compose, so $Z$ is never built. $h_{A \ominus B}(x) = \max\{(a - b) \cdot x\} = \max_a a \cdot x + \max_b b \cdot (-x) = h_A(x) + h_B(-x)$ (F.13), and if $a^*$, $b^*$ attain those maxima then $s_{A \ominus B}(x) = a^* - b^*$ (F.14). For a polygon the support point is the vertex with the largest dot product — a scan of $n$ vertices — so one GJK step costs $O(n_A + n_B)$ and the whole query is linear with a tiny constant. $\blacksquare$

What the check pins. gjk runs the two-dimensional iteration against Chapter 2's brute-force polygon distance on $500$ seeded convex pairs ($22$ of them intersecting): the distances agree to $4 \times 10^{-16}$, the witnesses realize the distance, the intersection verdict matches the separating-axis test, and no query needs more than $5$ iterations — well under the $n_A + n_B$ bound.

What parry2d adds. The engine the Rust crate delegates to runs this same loop with a bounding-volume hierarchy to skip far pairs, EPA (expanding polytope algorithm) to recover the penetration depth when the origin is inside $Z$, and shape-casting — GJK along a motion — for the swept query of Chapter 2's Collision::swept. Choset's one-sentence extension to polyhedra is to start from four points and keep a face instead of an edge.

Statement. The matrix above is $\partial\varphi/\partial q$; its determinant is $L_1 L_2 \sin\theta_2$; the arm is singular exactly when $\sin\theta_2 = 0$ — straight or folded. Choset's numbers: $L_1 = L_2 = 1$, $q = (\pi/4, \pi/2)$, $\dot q = (1, 0)^{\mathsf T}$ give $J = \begin{bmatrix} -\sqrt2 & -\sqrt2/2 \\ 0 & -\sqrt2/2 \end{bmatrix}$, $\det J = 1$, and $\dot x = (-\sqrt2, 0)^{\mathsf T}$.

Step 1 — differentiate entrywise. $\partial\varphi_1/\partial\theta_1 = -L_1\sin\theta_1 - L_2\sin(\theta_1 + \theta_2)$; $\partial\varphi_1/\partial\theta_2 = -L_2\sin(\theta_1 + \theta_2)$; $\partial\varphi_2/\partial\theta_1 = L_1\cos\theta_1 + L_2\cos(\theta_1 + \theta_2)$; $\partial\varphi_2/\partial\theta_2 = L_2\cos(\theta_1 + \theta_2)$. Read the columns: column $i$ is the tip velocity when joint $i$ alone turns at unit rate — every link beyond joint $i$ swings about it, so the column is the sum over those links of $L_k$ times the link's heading rotated by $90°$. That sentence is the implementation of jacobian, and it serves any number of links.

Step 2 — the determinant. Write $s_1 = \sin\theta_1$, $c_1 = \cos\theta_1$, $s_{12} = \sin(\theta_1 + \theta_2)$, $c_{12} = \cos(\theta_1 + \theta_2)$. Then $\det J = (-L_1 s_1 - L_2 s_{12})(L_2 c_{12}) - (-L_2 s_{12})(L_1 c_1 + L_2 c_{12}) = -L_1 L_2 s_1 c_{12} + L_1 L_2 s_{12} c_1 = L_1 L_2 \sin((\theta_1 + \theta_2) - \theta_1) = L_1 L_2 \sin\theta_2$ by the sine difference identity.

Step 3 — what rank loss means. At $\sin\theta_2 = 0$ the two columns are parallel (both are multiples of $(-\sin\theta_1, \cos\theta_1)$ when the arm is straight), so the image of $\dot q \mapsto J\dot q$ is a line perpendicular to the arm. Motion along the arm is instantaneously impossible — the tip is at the edge of the annulus and can only move tangentially. The arm has not broken; one direction is unavailable, and a planner that asks for it asks for $\dot q = J^{-1}\dot x$ with $\det J \to 0$: infinite joint speed. Chapters 17–18 meet this as a velocity limit that goes to zero; Chapter 21 as a rank condition.

Step 4 — Choset's numbers. At $q = (\pi/4, \pi/2)$: $\theta_1 + \theta_2 = 3\pi/4$, $s_1 = c_1 = \sqrt2/2$, $s_{12} = \sqrt2/2$, $c_{12} = -\sqrt2/2$. So $J_{11} = -\sqrt2/2 - \sqrt2/2 = -\sqrt2$, $J_{12} = -\sqrt2/2$, $J_{21} = \sqrt2/2 - \sqrt2/2 = 0$, $J_{22} = -\sqrt2/2$; $\det J = \sin(\pi/2) = 1$; and $J (1, 0)^{\mathsf T} = (-\sqrt2, 0)^{\mathsf T} = (-1.4142, 0)$ — the tip, at $(0, \sqrt2)$, moves straight left when the shoulder alone turns, as his figure 3.21 draws. The check jacobian asserts the four entries, the determinant and $\dot x$ to $10^{-12}$, and holds jacobian to central differences of Reach::fk on $300$ seeded arms to $10^{-7}$. $\blacksquare$

The velocity ellipse. The Jacobian Lens maps the unit circle of joint velocities through $J$: an ellipse with semi-axes the singular values $\sigma_1 \ge \sigma_2$ of $J$ and area $\pi\sigma_1\sigma_2 = \pi|\det J|$ — Yoshikawa's manipulability. At $\theta_2 = 10°$ (any $\theta_1$, any $L_1 = L_2 = 1$): $\sigma_1\sigma_2 = \sin 10° = 0.1736$, $\sigma_1^2 + \sigma_2^2 = \|J\|_F^2 = 3 + 2\cos 10° = 4.9696$, so $\sigma_1 = 2.228$, $\sigma_2 = 0.078$ and the ellipse is 28.6 times longer than it is wide; at $\theta_2 = 0$ it is a segment. Exercise 4 asks for this number before the widget shows it.

Example 3.8.2, the polygon. A point fixed at $r = (r_1, r_2)$ on a body with configuration $q = (q_1, q_2, q_3) \in \mathbb{R}^2 \times S^1$ sits at $x = (q_1, q_2) + R(q_3)\, r$, with Jacobian

a $2 \times 3$ matrix of rank $2$ always: a rigid body in the plane has no singular configurations, and $\varphi^{-1}$ is one-to-many because $\dim \Q > \dim M$. The third column is the lever arm rotated by $90°$, the velocity of the point under pure rotation. bodyPointJacobian implements it and the same check differences bodyPointFk against it.

$J^{\mathsf T}$ is the force map. Power is coordinate-free: $\tau \cdot \dot q = f \cdot \dot x = f \cdot J\dot q$ for every $\dot q$, so $\tau = J^{\mathsf T} f$. The widget's $J^{\mathsf T}$ toggle shows a unit tip force becoming joint torques; Chapter 7 lifts workspace potentials through this map and Chapter 17 makes it dynamics.