Configuration Space and the Planning Problem

A configuration $q$ is a minimal set of parameters that fully specifies the pose of every rigid body in a robot. For a 2-DOF planar arm, $q = (\theta_1, \theta_2) \in [-\pi, \pi)^2$; for a free-flying quadrotor, $q \in SE(3)$. The set of all reachable configurations is the configuration space $\mathcal{C}$.

The key insight: once you map the robot into $\mathcal{C}$, the robot itself disappears. It becomes a single point $q$. Planning reduces to connecting two points in $\mathcal{C}$ without entering the forbidden region. Every collision-avoidance problem, from a 2R arm to a 50-DOF humanoid, shares this common abstraction. All downstream algorithms — grids, graphs, sampling — operate exclusively in $\mathcal{C}$.

Partition $\mathcal{C}$ into three mutually exclusive sets:

• $\mathcal{C}_{\text{obs}}$: configurations where the robot body intersects an obstacle (or itself).
• $\mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}}$: configurations where the robot is collision-free.
• Boundary $\partial\mathcal{C}_{\text{obs}}$: grazing contact.

A motion plan is a continuous path $\sigma: [0,1] \to \mathcal{C}_{\text{free}}$ with $\sigma(0) = q_{\text{start}}$ and $\sigma(1) = q_{\text{goal}}$.

Critical subtlety: $\mathcal{C}_{\text{obs}}$ has no closed-form expression for most robots. For a 2D point robot, it equals the workspace obstacle footprints. For anything else — a 6-DOF arm, a car — computing $\mathcal{C}_{\text{obs}}$ analytically is NP-hard. This is why planners resort to collision-checking samples rather than explicitly constructing $\mathcal{C}_{\text{obs}}$.

The dimension of $\mathcal{C}$ equals the robot's degrees of freedom (DOF):

Joint limits and kinematic constraints carve out further subsets. Notice that $\mathcal{C}$ is not always $\mathbb{R}^n$: revolute joints wrap around, giving $S^1$ factors; orientation in 3D lives in $SO(3)$, not $\mathbb{R}^3$. Planners must respect topology — naive Euclidean distance across a $\pm\pi$ joint boundary is wrong and creates phantom discontinuities in path cost.

A 2-DOF planar arm in workspace maps to a point in the 2D joint angle space; workspace obstacles become C-space obstacles after Minkowski expansion.

For a 2D point robot, $\mathcal{C}_{\text{obs}}$ is obtained by growing each workspace obstacle by the Minkowski sum with the robot's shape $\mathcal{A}$:

$\mathcal{C}_{\text{obs}} = \{ q \mid \mathcal{A}(q) \cap \mathcal{W}_{\text{obs}} \neq \emptyset \} = \mathcal{W}_{\text{obs}} \oplus (-\mathcal{A})$

For a circular robot of radius $r$, the inflation is simply a dilation by $r$ — each obstacle grows by $r$ and the robot shrinks to a point. This is the standard trick in 2D navigation: inflate the costmap by the robot's inscribed radius, then plan for a point robot.

For non-circular robots or arms, the Minkowski sum is expensive to compute in closed form. Modern planners skip the explicit computation entirely and instead sample $q$, forward-kinematics to workspace, then test geometric intersection — trading precomputation for fast per-sample checks using BVH trees or FCL.

Grid-based planners discretize $\mathcal{C}$ with resolution $\epsilon$. The number of cells scales as $(1/\epsilon)^n$ where $n = \dim(\mathcal{C})$. For a 6-DOF arm at $5^\circ$ resolution that's $72^6 \approx 1.4 \times 10^{11}$ cells — totally intractable.

More formally: a ball of radius $r$ in $\mathbb{R}^n$ has volume $V_n r^n$ where $V_n = \pi^{n/2}/\Gamma(n/2+1)$. To cover $\mathcal{C}_{\text{free}}$ with $\epsilon$-balls needs $\Theta((1/\epsilon)^n)$ samples. For $n \geq 6$, uniform grids are dead on arrival.

The curse motivates sampling-based planners (RRT, PRM) that replace exhaustive coverage with random exploration — they do not require covering the full space, only finding one valid path. The tradeoff: you lose completeness guarantees (only probabilistic completeness) but gain tractability in high $n$.

A collision checker answers: is $q \in \mathcal{C}_{\text{obs}}$? Modern checkers (FCL, libccd, Bullet) have three layers:

1. Broad phase — bounding volume hierarchies (AABB, OBB, k-DOP) cull obviously non-colliding pairs in $O(\log n)$.
2. Narrow phase — GJK/EPA for convex pairs, mesh-mesh for non-convex shapes.
3. Continuous collision checking (CCC) — tests the swept volume of a segment $[q_a, q_b]$, critical for validating edges in a roadmap.

import yourdfpy, fcl

def is_collision_free(q, robot, scene):
    robot.set_joint_positions(q)          # FK
    for geom_pair in scene.broadphase(robot):
        if geom_pair.narrow_check():      # GJK
            return False
    return True

Collision checking dominates planner runtime — up to 90% of wall time in dense environments. Batch GPU checkers (Isaac Gym, CuRobo) exploit parallelism to amortize cost across thousands of simultaneous queries.

The basic motion planning problem (BMPP): given $\mathcal{C}$, $\mathcal{C}_{\text{obs}}$, $q_{\text{start}} \in \mathcal{C}_{\text{free}}$, $q_{\text{goal}} \in \mathcal{C}_{\text{free}}$, find a continuous path $\sigma: [0,1] \to \mathcal{C}_{\text{free}}$ or certify none exists.

Variants add cost:

• Optimal motion planning: minimize $\int_0^1 c(\sigma(t), \dot{\sigma}(t))\,dt$ (path length, energy, time).
• Kinodynamic planning: $\dot{q} = f(q, u)$ — the robot has dynamics and bounded controls.
• Multi-query planning: answer many $( q_s, q_g )$ pairs sharing the same $\mathcal{C}_{\text{obs}}$.

For the rest of this cursus, we focus on the geometric case (no dynamics) but flag where dynamics change the picture. Appreciating $\mathcal{C}$ is the prerequisite: without it, Dijkstra, A*, PRM, and RRT are just opaque algorithms. With it, they are all instances of graph search or sampling in $\mathcal{C}_{\text{free}}$.