Moving obstacles, robot teams, manipulation, closed chains, coverage, and optimal paths — the many lives of the basic planning problem.
The planning problem from Chapters 4 and 5 is clean: one robot, static obstacles, find a collision-free path from start to goal. Solve it once, execute it, done. Real planning problems are messier in at least seven distinct ways, and Chapter 7 of LaValle’s book tackles all seven.
Imagine you’re programming a warehouse robot. The obstacles aren’t static — other robots are moving through the same space. Your robot isn’t alone — a fleet of twenty robots needs to reach twenty different goals simultaneously. The robot needs to pick things up and put them down — grasping changes what counts as a collision. Two arms of the same robot are connected at the base — they form a closed kinematic chain with constraints. Your task isn’t to reach a goal — it’s to sweep every aisle. And when you plan a path, you want the shortest one, not just any valid one.
Each of these complications breaks one assumption of the basic problem. Each has its own C-space formulation. And each, remarkably, can be reduced to familiar search or sampling in a modified space.
| Extension | What changes | New C-space |
|---|---|---|
| Time-varying obstacles | Obstacles move | C × T (space-time) |
| Multiple robots | Many robots, many goals | C1 × C2 × … × Cn |
| Manipulation planning | Robot grasps objects | Discrete modes + continuous C-space per mode |
| Closed kinematic chains | Joints form loops | Constraint manifold inside C |
| Folding problems | Distance constraints | Feasible subspace of C |
| Coverage planning | Visit every point | Same C, different objective |
| Optimal planning | Minimize cost | Same C, add cost-to-go |
Each dot is a planning problem variant. Hover (or tap) to see what changes from the basic problem.
Picture a robot trying to cross a busy intersection. Cars move through at known speeds and schedules. The robot needs to find a path that avoids every car — but the cars’ positions depend on when the robot is there, not just where. Static C-space has no way to represent this. We need to add time.
The solution is elegant: promote time from a background parameter to an explicit dimension of C-space. The new space is C × T, where C is the original configuration space and T is the time axis. A path through C × T is a function (q(t), t) — a curve that specifies both position and the exact moment the robot is at that position. LaValle calls this the space-time configuration space.
In 2D workspace, C = R2. Adding time gives C × T = R2 × R, a 3D space. A moving disk obstacle with radius r centered at (xo(t), yo(t)) becomes a swept tube in this 3D space. If the disk moves at constant velocity, the tube is a tilted cylinder. If it accelerates or turns, the tube curves. The robot’s path must avoid all tubes.
In C × T, the robot’s velocity enters the picture. Moving from (q, t1) to (q′, t2) in space-time corresponds to covering configuration-space distance Δq = q′ − q in wall-clock time Δt = t2 − t1. The slope of the path in space-time is the robot’s velocity. If the robot has a maximum speed vmax, then Δq / Δt ≤ vmax — this limits the slope of any valid path in space-time.
With a finite time horizon Tmax, the space-time C-space is C × [0, Tmax]. If obstacle motions are known in advance (a planned schedule), you can build the obstacle tubes explicitly and run a standard planner — RRT or PRM — in the augmented space. The only modification: edges must have non-decreasing t-coordinates.
For predictable obstacles (repeating schedules, known trajectories), the planner can precompute which tubes exist and sample freely in C × T. For unpredictable obstacles (humans wandering, other uncontrolled agents), the planner must replan online, treating the current time and configuration as the new start state.
Left: workspace at the current time slice (red obstacle moves left-right). Right: the space-time view (t is vertical). Drag the time slider to see how the robot’s path threads between obstacle tubes.
Now imagine two robots sharing the same workspace. Each has its own configuration. Robots can collide with each other, not just with fixed obstacles. Planning for them independently risks collision — but planning for them jointly requires thinking about both robots simultaneously.
The right way to think about this is to form the composite C-space: the Cartesian product of each robot’s individual C-space. If robot 1 has C-space C1 and robot 2 has C-space C2, the composite C-space is C1 × C2. A single point (q1, q2) in this composite space completely specifies the configuration of both robots at once.
The composite C-space has two types of obstacle regions. External obstacles: configurations where either robot collides with a fixed environment obstacle. These are the same as the single-robot Cobs lifted into the composite space — robot 1’s Cobs × C2 ∪ C1 × robot 2’s Cobs. Interaction obstacles: configurations where the two robots collide with each other. This is harder — it depends on both (q1, q2) jointly and cannot be expressed as a product.
where Cinteraction = {(q1, q2) : A1(q1) ∩ A2(q2) ≠ ∅} is the set of configurations where the two robots physically overlap.
Consider two robots that must pass each other in a narrow corridor. If robot 1 is at position x1 = 0.3, robot 2 must be at x2 > 0.7 to avoid collision. The feasible configurations of robot 2 depend on where robot 1 is — the problem is inherently coupled. In the composite C-space, the corridor constraint appears as a diagonal strip of free space, and the robots must coordinate along this strip.
Each axis is one robot’s 1D position in the corridor. Gray = one robot blocks the other. Drag both sliders. The green dot shows the current composite configuration.
Planning in the full composite C-space is complete — if a solution exists, the planner will find it. But in the full composite space, a warehouse fleet of 20 robots has a C-space of 60+ dimensions. Exact planners are hopeless. Sampling-based planners struggle. We need smarter strategies.
LaValle §7.2 describes two major families: decoupled planning and prioritized planning. Both sacrifice completeness for tractability. Both are widely used in real systems.
The simplest approach: plan for each robot independently, ignoring the other robots, then execute the plans simultaneously and hope they don’t collide. This is almost always wrong — the plans will clash whenever two robots need to pass through the same point.
A better decoupled approach is path-velocity decomposition. First, plan geometric paths for each robot (ignoring timing). Second, assign velocities along those paths so that robots never occupy the same point at the same time. The geometric planning is cheap (each robot’s path lives in its own C-space). The velocity scheduling is a 1D coordination problem per pair of paths.
Prioritized planning (Erdmann and Lozano-Perez, 1987) is subtler and more powerful. Assign a priority order to the robots. Plan for robot 1 freely in its own C-space. Then plan for robot 2 in its own C-space, but treating robot 1’s path as a moving obstacle — exactly the time-varying planning problem from Chapter 1. Then plan for robot 3 treating robots 1 and 2 as moving obstacles. And so on.
Each robot in the priority order faces a single-robot planning problem with time-varying obstacles. In C × T, the already-planned robots carve out tubes, and the new robot must thread between them. The composite C-space is never formed explicitly — we solve n single-robot problems instead of one n-robot problem.
Prioritized planning is not complete. Here is the canonical failure: two robots in a narrow corridor, each needing to pass through the other’s start position. Robot 1 gets the corridor. Robot 2 sees robot 1 occupying the corridor indefinitely and has no path. Reordering priorities doesn’t help — both directions fail. A complete planner in the composite C-space would find that the only solution requires both robots to move simultaneously in a coordinated dance that prioritized planning cannot express.
A robot arm needs to move a box from one table to another. This is not just motion planning — the robot must grasp the box (change the world state), carry it (plan with the box attached), and release it (change the world state again). Each grasp or release changes the effective C-space of the system. This is manipulation planning.
The key new idea is the mode. A mode encodes the discrete state of the world: which objects the robot is holding, which objects are in contact with each other, which grasp is active. In the box example, there are two modes: “arm free, box on table A” and “arm holding box.” Within each mode, the C-space and the obstacles are fixed — standard sampling-based planning applies. Between modes, a mode switch happens: the robot grasps or releases an object.
Formally, the manipulation planning problem is defined by:
A solution is a sequence of mode/path pairs: (m1, γ1), switch to m2, (m2, γ2), switch to m3, … ending in the goal mode with the goal configuration reached.
The set of configurations where the robot can legally grasp an object is called the grasp region G ⊂ Cfree. Similarly, the set of configurations where an object can be legally placed (on a surface, without collision) is the placement region P ⊂ Cfree. A complete manipulation plan must have γ1 ending in G (to execute the grasp), and γ2 ending in P (to execute the release).
Discrete mode graph: each node is a mode, each edge is a mode switch. Click a mode to see its C-space geometry (simplified 1D).
Most robot arms are open chains: a base, links, joints, end-effector, in a linear sequence. The configuration is simply the vector of joint angles, and forward kinematics gives you the end-effector pose. Easy. But what if the end-effector is constrained to touch the environment? Or if two arms grasp the same object? The chain has a loop — it is a closed kinematic chain.
A human hand grasping a door handle is a closed chain: the arm (open chain), the handle (fixed in space), and the constraint that the hand must maintain contact with the handle. A two-armed robot carrying a rigid box is a closed chain: each arm is an open chain, but they share an end-constraint at the box. Even a four-bar linkage in a mechanism is a closed chain.
For an n-DOF open chain with a 6D closure constraint, the constraint g(q) = 0 ∈ R6 defines (generically) an (n−6)-dimensional manifold inside C = Rn. With 7 DOF and 6 constraints, the closure manifold is 1-dimensional — a curve in C. With 8 DOF, it is a 2D surface. Planning must stay on this manifold.
This creates a fundamentally different planning challenge. In standard planning, Cfree is an open subset of C and sampling is easy — draw random q, check collision. On the closure manifold, random q almost never satisfies the constraint. Uniform samples lie off the manifold with probability one. The planner must either project samples onto the manifold or sample on the manifold directly.
The most common approach: sample q randomly in C, then solve the constraint equations numerically using Newton-Raphson iteration to find the nearest point on the manifold. If Newton’s method converges (not guaranteed), you get a valid configuration. The projection may fail (no convergence) or land in a collision configuration. Successful projections are then used as nodes in a PRM or RRT.
where J+ is the pseudoinverse of the constraint Jacobian ∂g/∂q. Each Newton step reduces the constraint violation. Iterate until ‖g(q)‖ < ε.
A 2-DOF chain with a 1D closure constraint. The constraint is that the end-effector y-coordinate = 0.5. The valid configurations form a 1D curve (teal). Drag the sliders — the projection button snaps q to the nearest manifold point.
Every planning problem so far has asked: find a path from start to goal. Coverage planning asks something different: find a path that visits every point in the workspace. Lawn mowing, floor cleaning, painting, crop inspection, mine sweeping — all are coverage problems. The goal is not a single configuration but a guarantee that no part of the environment is left unvisited.
The workspace is typically discretized into a grid of cells. The robot (modeled as a point or disk) covers a cell when it passes through it. The coverage problem asks for a path that covers all free cells. This is related to the Chinese Postman Problem (traverse every edge of a graph) and the Travelling Salesman Problem (visit every vertex), both NP-hard in general.
The simplest complete coverage strategy is the boustrophedon decomposition (from the Greek for “as the ox plows”). Divide the free space into vertical strips. Within each strip, the robot moves up and down in parallel rows. At the top of one row, it shifts one row-width and moves back down. The result is a lawnmower pattern: complete coverage of each strip guaranteed, no cell visited twice.
When obstacles are present, boustrophedon decomposes the workspace at each vertical edge of an obstacle into separate cells. Each cell gets its own lawnmower pass. The robot moves between cells by connecting the endpoint of one cell’s sweep to the start of the next. This is complete: if the workspace is decomposable into boustrophedon cells, full coverage is guaranteed.
Wavefront propagation (also called potential field coverage) takes a different approach. Initialize a numeric label on every free cell. Start with 0 at the goal (or the last visited cell). Propagate outward: neighbors of 0 get label 1, their unvisited neighbors get 2, and so on — a breadth-first flood fill. The robot then follows the gradient of increasing labels to reach every cell. When all cells are labeled, the gradient field provides a complete coverage path.
Wavefront propagation is complete (every reachable cell gets a label), produces no repeated visits in open spaces, and naturally handles arbitrary obstacle shapes. Its weakness: the generated path can be inefficient, with many turns and backtracking when obstacles create isolated pockets.
Draw obstacles (click cells to toggle walls), choose algorithm, then Run. Watch the robot cover every free cell. Controls: Boustrophedon sweeps rows, Wavefront propagates a potential field.
| Algorithm | Completeness | Repeated visits | Path efficiency |
|---|---|---|---|
| Boustrophedon | Complete (if decomposable) | Low (transitions only) | High in open rooms |
| Wavefront | Complete (all reachable cells) | None in theory | Medium (many turns) |
| Random bounce | Probabilistically complete | High | Low |
| Spanning tree coverage | Complete | None | Optimal in grid graphs |
Standard RRTs and PRMs find some valid path. They make no promises about the path’s length or energy cost. In practice, the first path RRT finds often makes unnecessary detours — it bounces through Cfree wherever samples happened to fall. For a surgical robot or autonomous vehicle, a jagged, inefficient path is not acceptable. We want the best path, not just any path.
Optimal motion planning minimizes a cost functional over all valid paths. The cost might be path length, time, energy, control effort, or any combination. The problem is to find the minimum-cost path from start to goal in Cfree. This is much harder than feasibility-only planning.
Karaman and Frazzoli (2011) proved that a small modification to RRT makes it asymptotically optimal: as the number of samples n → ∞, the path found converges to the optimal path almost surely. The modification has two parts:
1. Near-neighbor rewiring radius. Instead of connecting each new sample only to its nearest existing node, RRT* considers all nodes within a ball of radius r(n) = γRRT* (log n / n)1/d, where d is the C-space dimension and γRRT* is a constant. This radius shrinks as n grows but encompasses a growing number of nodes due to increasing density.
2. Rewiring. After adding a new node qnew, RRT* checks: for each node qnear in the ball, would connecting qnear to qnew and then through qnew’s current tree path give qnear a lower cost-to-root? If yes, rewire: make qnew the parent of qnear. This propagates cost improvements backward through the tree.
The same analysis applies to PRM. Standard PRM connects each sample to all neighbors within a fixed radius — yielding a graph where shortest-path queries give good but not optimal paths. PRM* uses the same shrinking-radius formula: rPRM*(n) = γPRM* (log n / n)1/d. With this radius, as n → ∞, the graph densifies exactly fast enough that the shortest path through it converges to the true shortest path in Cfree.
Both planners run from the same start (green) to the same goal (red) with the same random seed. Watch how RRT* rewires the tree to reduce total path length over time. The cost counter updates in real time.
The seven extensions are not independent tricks. Real planning problems combine several simultaneously. A warehouse robot fleet (multi-robot) operates in a space with moving human workers (time-varying), picks up packages (manipulation), and needs to cover every aisle efficiently (coverage) with minimum driving distance (optimal). The full problem sits at the intersection of multiple Chapter 7 extensions.
Autonomous driving
Robotic surgery
Protein folding
Floor cleaning robot
For any extension, LaValle’s framework gives a systematic recipe:
Chapter 7 is a bridge. It takes the sampling-based planning machinery from Chapters 4–5 and shows where it reaches — and where it needs augmentation. The extensions point forward to the rest of the book.
| Extension | Leads to | Where in LaValle |
|---|---|---|
| Time-varying planning | Kinodynamic planning, trajectory optimization | Ch 13–14 |
| Multiple robots | Game theory, decentralized control | Ch 9 (game theory) |
| Manipulation | Task and motion planning (TAMP), hybrid systems | Research literature |
| Closed chains | Constraint satisfaction, retraction methods | Ch 7.4, research |
| Coverage | Sensor placement, inspection planning | Research literature |
| Optimal planning | Optimal control, Pontryagin minimum principle | Ch 14–15 |
Every extension in Chapter 7 assumes the robot has complete information: it knows the obstacle positions, the other robots’ trajectories, the mode structure. Real systems operate with uncertainty. A camera gives noisy depth readings. Other robots may behave unpredictably. The true configuration may be only partially observed.
Planning under uncertainty — where the state is not fully known — requires the information space (I-space), which LaValle introduces in Chapter 11. The I-space is to planning under uncertainty what C-space is to classical planning: the right abstract space in which to formulate the problem.
“The most important contributions of motion planning research are the ideas — the conceptual frameworks and the understanding of complexity — rather than specific algorithms.”
— Steven M. LaValle, Planning Algorithms (2006)