Planning for Many Bodies: Multi-Robot, Manipulation, and Kinodynamic Previews

Composite configuration spaces and the robot–robot C-obstacle, decoupled and prioritized multi-robot planning and why both are incomplete, conflict-based search proved optimal on a grid, manipulation planning as transit and transfer modes on a manipulation graph, and a one-page preview of forward propagation for Hitch — plus OMPL and MoveIt as the systems in which Part III is infrastructure.

The problem: two free paths are not a free plan

Two Rustys in the Apartment. Rusty 1 wants to go from room A to room C; Rusty 2 wants the reverse. Each is handed to the single-robot planner of Chapter 6 — A* on the inflated lattice — and each gets back a path that touches no wall. Execute both at once.

They meet head-on in the corridor. Nothing in either planner was wrong; nothing in either planner could have been right. Each one checked its robot against the walls and never against the other robot, because the other robot was not a configuration it ever saw. The collision lives in a space neither planner searched: the set of pairs of configurations, where the second body is an obstacle that moves.

That space is the product $\Q = \Q_1 \times \Q_2$, four-dimensional for two discs, and its obstacle has two parts. The lifted walls, which each robot already knew about, and a new set — the configurations where the two bodies overlap — which neither did. The hook, in numbers: at the composite configuration $q = (1.0, 1.0, 1.8, 1.4)$ in a corridor of height $2$, each disc of radius $0.5$ is $0.5$ clear of both walls, and their centers are $\sqrt{0.8^2 + 0.4^2} = 0.8944$ apart. Each robot's own check passes. The composite check does not: $0.8944 < 1.0$, a penetration of $0.1056$.

Building intuition: one space, many bodies

The amber hole that moves

The top panel is two discs of radius $0.5$ in a corridor with one alcove, swapping ends. The bottom-left panel is robot 2's slice of the four-dimensional composite space: the two coordinates of robot 2, with robot 1 frozen where it is. Slate is where robot 2 alone would hit a wall — the Chapter 4 picture, a disc robot inflated against the corridor. Amber is new: the disc of radius $r_1 + r_2 = 1$ around robot 1, the robot–robot C-obstacle $\QO_{12}$ cut at robot 1's current position. Drag robot 1 and the amber hole follows. That is the whole difference between one robot and two: a C-obstacle that is a function of the other robot's configuration.

At the default width of 3.2 radii the amber disc spans the corridor's full height. Below four radii of width the two discs cannot pass each other anywhere in the corridor, so whichever robot is in the corridor blocks it entirely, and the only resolution is the alcove: someone steps aside, the other passes, the first steps back out. Try the four planner chips.

Decoupled. Plan each robot alone, then coordinate speeds along the two fixed paths. The bottom-right panel is the coordination diagram $[0,1]^2$ of path parameters: amber where the two bodies, each at its own parameter, overlap. A velocity schedule is a monotone path from $(0,0)$ to $(1,1)$ — neither robot drives backward along its path. Both paths planned alone are the corridor axis, so the obstacle is a band along the anti-diagonal $s_1 + s_2 \approx 1$, covering 39.0 percent of the square, and no monotone path crosses it. No wait of any length fixes this.

Prioritized 1→2 and 2→1. Robot 1 plans alone in configuration–time space; robot 2 plans with robot 1 as a moving obstacle. Robot 1's trajectory is the axis, the alcove is of no use to a robot planning alone, and robot 2 finds no trajectory within its budget. Reverse the order and the same thing happens with the roles swapped. The prioritized planner fails in both orders.

Coupled. RRT-Connect from Chapter 12 on the product $(\mathbb R^2)^2$, with a free-space predicate that checks each disc against the walls and then the pair against each other. It finds the detour: one robot into the alcove, the other past, the first out. On seed 14 the trees merge at attempt 2562 with 60 nodes between them, after 3278 composite checks of which 199 died on the pair term alone. The sample count is the price of four dimensions; the pair term is the price of two bodies. Both are reported, not hidden.

The misconception this widget exists to kill: "plan each robot separately and add a wait." The swap problem has no wait that works. "Robot 1 waits in the alcove" is a different path for robot 1, and only the composite space can express it.

Modes, not a bigger robot

Reach moves a puck from placement A to placement B on the Workbench. The puck is slate when it rests on the table and orange when the arm holds it, and the two colors are two different robots. With the puck at rest it is an obstacle, and the arm moves on a transit manifold: the amber region on the left torus is Reach's C-obstacle with one more body in the scene. With the puck held it is part of the robot, rigidly attached to the hand, and the arm moves on a transfer manifold: the amber region on the right torus is a different shape, because the robot has a different shape.

Both manifolds have the dimension of the arm alone — two — inside a composite space of dimension four. The system can leave one only at a configuration that lies on both: the arm holding the puck at a placement, which is a grasp. The graph on the right has a node for every feasible (placement, grasp) pair and an edge for every transit (same placement, different grasp: a regrasp) or transfer (same grasp, different placement). A manipulation plan is a path in this graph.

The headline slider adds grasps. With the front grasp alone, placement B — behind the bar — admits no grasp configuration at all, and the graph is disconnected: regrasping is impossible because there is only one grasp. With two grasps the graph is still disconnected if there is nowhere to set the puck down in between; add placement C and the plan is transit, transfer, transit, transfer — carry the puck from A to C with the front grasp, release, regrasp from the side, carry it to B. Grey dashed edges were never planned for real; only the four on the chosen path were, out of seven. That is the lazy two-level PRM of §7.5.3, and the counter is the number Exercise 4 asks about.

The misconception killed: "manipulation planning is path planning with a bigger robot." It is a union of lower-dimensional manifolds, and switching happens only at their intersections.

Conflicts, not the joint space

A room with two pillars, three Rustys crossing from the west wall to the east wall in reversed order. The root of the tree plans every agent alone with Chapter 6's A* over (cell, time). Expanding a node means finding the first conflict in its paths — two agents in one cell at one time, or swapping an arc — and spawning two children, each forbidding that cell to one of the two agents at that time, each re-planning only that agent. Nodes are popped in order of the sum of individual costs, so the first conflict-free node popped has the smallest sum any conflict-free plan can have.

Watch the two numbers in the readout. On the three-agent room the constraint tree has three nodes and two expansions; a coupled A* over the joint state, which must consider up to $5^3 = 125$ joint moves per expansion, generates 1412 successors to find the same cost. Slide to five agents: the tree grows to eleven nodes, while the joint search's branching factor is $3125$ and it is not run. The tree's size tracks conflicts, which in an apartment are few; the joint search's size tracks agents, exponentially.

The misconception killed: "optimal multi-agent paths require searching the joint space."

The mathematics

Definitions

Structure and price of the composite C-obstacle

Decoupled and prioritized planning are incomplete

Conflict-based search is complete and optimal

A manipulation plan is a path in a union of manifolds

Steer-free planners inherit Theorem 7.4.3

The algorithm

Three of this chapter's algorithms are Chapters 11–12's with a different space; the rest are new coordination schemes on top of them.

The two-disc test, by hand

Corridor $y \in [0, 2]$, two discs of radius $0.5$, composite $q = (x_1, y_1, x_2, y_2)$.

At $q = (1.0, 1.0, 1.8, 1.4)$. Disc 1's clearance to the walls is $\min(1.0 - 0.5,\ 2 - 1.0 - 0.5) = 0.5$; disc 2 spans $y \in [0.9, 1.9] \subset (0, 2)$. Both are free alone. The centers are $\sqrt{0.8^2 + 0.4^2} = \sqrt{0.80} = 0.8944$ apart, less than $r_1 + r_2 = 1$: $q \in \QO_{12}$, collision, penetration $0.1056$.

At $q' = (1.0, 1.0, 2.0, 1.4)$. $\sqrt{1.0^2 + 0.4^2} = \sqrt{1.16} = 1.0770 > 1$: free.

Along the edge $q' \to q$, only $x_2$ moves, from $2.0$ to $1.8$. The edge enters $\QO_{12}$ where $(x_2 - 1)^2 + 0.16 = 1$, i.e. $x_2 = 1 + \sqrt{0.84} = 1.9165$. Chapter 11's subdivision schedule at step $0.1$ on an edge of length $0.2$ has one interior check, the midpoint $x_2 = 1.9$, where $\|p_1 - p_2\| = \sqrt{0.81 + 0.16} = 0.9849 < 1$: rejected at the first interior check. (steer tests the far endpoint $q$ first and would already have said no.)

Prioritized slice. Freeze robot 1 at $(1.0, 1.0)$. Robot 2's slice C-obstacle is the disc of radius $1$ about $(1, 1)$, spanning $y \in [0, 2]$ — the full corridor height at $x_2 = 1$. Scanning robot 2's own free band $y \in [0.5, 1.5]$ at $x_2 = 1$, every position collides: blocked fraction $1.00$. Robot 2 must wait or fail: Derivation 2, in numbers.

CBS on the swap corridor, by hand

The grid is a corridor of seven cells, $(0,0)$ to $(6,0)$, with one alcove cell $(3,1)$ above the middle; agent 1 goes from $(0,0)$ to $(6,0)$, agent 2 the reverse. Every number below is produced by Cbs on this instance and pinned by a check.

Root. Each agent planned alone takes the axis in six moves: $\mathrm{SIC} = 6 + 6 = 12$. The first conflict is a vertex conflict at cell $(3,0)$, $t = 3$ — the middle, where two robots driving at the same speed from opposite ends must meet.

Depth 1. Two children, each forbidding $(3,0)$ at $t = 3$ to one agent. The constrained agent waits one step before the middle: $\mathrm{SIC} = 13$ in both. Each now has an edge conflict at $t = 4$ — the agents try to swap the arc into the middle cell, which no vertex constraint forbids. This is why edge conflicts are not optional.

Depths 2 and 3. Each edge conflict splits into two arc constraints; the re-planned agent waits once more, $\mathrm{SIC} = 14$, and the conflict moves one cell and one time step along the corridor. At depth 3 the costs reach $15$ and the tree has grown to 23 nodes, of which CBS expands 12 in order of cost.

Depth 4. The first conflict-free node popped: agent 1 drives $0, 1, 2$, waits, then $3, 4, 5, 6$ — seven steps; agent 2 drives $6, 5, 4, 3$, steps up into the alcove $(3,1)$, steps back down after agent 1 has passed beneath, and continues $2, 1, 0$ — eight steps. $\mathrm{SIC} = 15$, which the brute-force joint A* confirms is the minimum: no plan that keeps both robots in the corridor can work, and every plan that uses the alcove costs at least two extra steps for the robot that detours plus one wait for the other. The joint search needed 19 expansions and 138 joint successors to say the same thing; here it is cheap, because there are two agents. With five it is not run.

Implementation in Rust

The multi crate adds no planner. It adds a space (Composite), a free-space predicate (MultiBody), three coordination schemes (prioritized, coordination, cbs), a mode graph (manip) and a propagation trait (propagate), and imports everything else from manifold, collide, robots, search and sampling. The TypeScript port the widgets run is web/lib/multi/ with the same module names.

use manifold::{Chart, Manifold};
use sampling::FreeSpace;

/// 𝒬 = A × B. dist is the ℓ² combination so Theorem 7.4.1's balls are balls; interpolate is
/// componentwise, which is why Chapter 11's straight-line Steer works unchanged on the product.
pub struct Composite<A: Manifold, B: Manifold> { pub a: A, pub b: B }

impl<A: Manifold, B: Manifold> Manifold for Composite<A, B> {
    type Point = (A::Point, B::Point);
    fn dim(&self) -> usize { self.a.dim() + self.b.dim() }
    fn dist(&self, p: &Self::Point, q: &Self::Point) -> f64 {
        self.a.dist(&p.0, &q.0).hypot(self.b.dist(&p.1, &q.1))
    }
    fn interpolate(&self, p: &Self::Point, q: &Self::Point, t: f64) -> Self::Point {
        (self.a.interpolate(&p.0, &q.0, t), self.b.interpolate(&p.1, &q.1, t))
    }
    fn sample(&self, rng: &mut impl rand::Rng) -> Self::Point { (self.a.sample(rng), self.b.sample(rng)) }
}

/// m copies of one chart — three Rustys. `Product<M, N>` is the const-generic form of the same.
pub struct Product<M: Chart, const N: usize> { pub factor: M }

/// Two discs overlap iff the center distance is below r₁ + r₂: Chapter 4's disc C-obstacle with
/// both bodies moving. In Rust this is one parry2d distance query, the same one the world check makes.
pub fn discs_collide(p1: [f64; 2], r1: f64, p2: [f64; 2], r2: f64) -> (bool, f64) {
    let d = ((p1[0] - p2[0]).powi(2) + (p1[1] - p2[1]).powi(2)).sqrt();
    (d <= r1 + r2, d)
}

/// Free space of m bodies: each against the world (Chapter 2), then every pair. Two counters,
/// because the lab reports how much of the bill is pairwise.
pub struct MultiBody<'w, M: Manifold> {
    pub bodies: Vec<Body<'w>>, pub space: M,
    pub world_checks: u64, pub pair_checks: u64, pub pair_only_rejections: u64,
}

impl<'w, M: Manifold<Point = Vec<[f64; 2]>>> FreeSpace<M> for MultiBody<'w, M> {
    fn is_free(&mut self, q: &M::Point) -> bool {
        // World first: cheaper, and where most random composite samples die.
        for (b, qi) in self.bodies.iter().zip(q) { self.world_checks += 1; if !b.world.is_free(qi) { return false; } }
        for i in 0..q.len() { for j in i + 1..q.len() {
            self.pair_checks += 1;
            if discs_collide(q[i], self.bodies[i].r, q[j], self.bodies[j].r).0 { self.pair_only_rejections += 1; return false; }
        } }
        true
    }
}

The two-disc test is the unit test and the worked example at once:

use multi::composite::{discs_collide, MultiBody};
use sampling::steer::{check_schedule, EdgeCheck};

fn main() {
    let r = 0.5;
    let (q1, q2) = ([1.0, 1.0], [1.8, 1.4]);
    let (hit, d) = discs_collide(q1, r, q2, r);
    println!("q : dist {d:.4}, {}, penetration {:.4}", if hit { "collide" } else { "free" }, 2.0 * r - d);
    let (hit, d) = discs_collide(q1, r, [2.0, 1.4], r);
    println!("q': dist {d:.4}, {}", if hit { "collide" } else { "free" });
    // Boundary along the edge q' → q: (x₂ − 1)² + 0.4² = 1.
    println!("boundary x2 = {:.4}", 1.0 + (1.0 - 0.16f64).sqrt());
    // Subdivision at step 0.1 on an edge of length 0.2: one interior check, the midpoint.
    let ts = check_schedule(EdgeCheck::Subdivision { step: 0.1 }, 0.2);
    let x2 = 2.0 - 0.2 * ts[0];
    let (hit, d) = discs_collide(q1, r, [x2, 1.4], r);
    println!("first interior check x2 = {x2:.1}: dist {d:.4} -> {}", if hit { "rejected" } else { "accepted" });
    // Robot 2's slice at x₂ = 1 with robot 1 pinned: scan its own free band.
    let blocked = (0..400).filter(|k| { let y = 0.5 + (*k as f64 + 0.5) / 400.0; discs_collide(q1, r, [1.0, y], r).0 }).count();
    println!("slice at x2 = 1.0: {:.2} blocked", blocked as f64 / 400.0);
}

#[test]
fn reproduces_two_disc_test() {
    let (hit, d) = discs_collide([1.0, 1.0], 0.5, [1.8, 1.4], 0.5);
    assert!(hit && (d - 0.8944).abs() < 1e-4 && (1.0 - d - 0.1056).abs() < 1e-4);
    let (hit, d) = discs_collide([1.0, 1.0], 0.5, [2.0, 1.4], 0.5);
    assert!(!hit && (d - 1.0770).abs() < 1e-4);
    assert!((1.0 + 0.84f64.sqrt() - 1.9165).abs() < 1e-4);
    let (hit, d) = discs_collide([1.0, 1.0], 0.5, [1.9, 1.4], 0.5);
    assert!(hit && (d - 0.9849).abs() < 1e-4);
    // MultiBody agrees with the closed form on 1 000 seeded composite configurations (parry2d vs. hypot).
    let mut rng = rand::rngs::SmallRng::seed_from_u64(14);
    let mut mb = MultiBody::two_discs(&CORRIDOR, 0.5);
    for _ in 0..1000 { let q = mb.space.sample(&mut rng); assert_eq!(mb.is_free(&q), MultiBody::closed_form(&CORRIDOR, &q, 0.5)); }
}

Expected output:

q : dist 0.8944, collide, penetration 0.1056
q': dist 1.0770, free
boundary x2 = 1.9165
first interior check x2 = 1.9: dist 0.9849 -> rejected
slice at x2 = 1.0: 1.00 blocked

Conflict-based search needs the time-expanded grid as a Chapter 6 Graph, and nothing else new:

use search::{astar, Graph, Edge};
use search::grid::{Grid, GridIdx};

pub struct Conflict { pub a: usize, pub b: usize, pub cell: GridIdx, pub t: u32, pub edge: Option<(GridIdx, GridIdx)> }
pub struct Constraint { pub agent: usize, pub cell: GridIdx, pub t: u32, pub from: Option<GridIdx> }

/// One agent's search space: (cell, time), five moves, minus what its constraints delete.
struct TimedGrid<'g> { grid: &'g Grid, goal: GridIdx, vertex_ban: HashSet<(GridIdx, u32)>, edge_ban: HashSet<(GridIdx, GridIdx, u32)>, last_goal_ban: i64, horizon: u32 }

impl<'g> Graph for TimedGrid<'g> {
    type Node = (GridIdx, u32);          // t = u32::MAX marks "resting at the goal from here on"
    fn neighbors(&self, &(cell, t): &Self::Node) -> Vec<Edge<Self::Node>> {
        let mut out = Vec::new();
        if cell == self.goal && t as i64 >= self.last_goal_ban { out.push(Edge { to: (cell, u32::MAX), cost: 0.0 }); }
        if t >= self.horizon { return out; }
        for nb in self.grid.moves_with_wait(cell) {
            if self.vertex_ban.contains(&(nb, t + 1)) { continue; }
            if nb != cell && self.edge_ban.contains(&(cell, nb, t + 1)) { continue; }
            // A wait at the goal is free; everything else costs one time step.
            let cost = if nb == self.goal && cell == self.goal { 0.0 } else { 1.0 };
            out.push(Edge { to: (nb, t + 1), cost });
        }
        out
    }
}

pub struct CtNode { pub constraints: Vec<Constraint>, pub paths: Vec<Vec<GridIdx>>, pub cost: u32 }

pub struct Cbs<'g> { grid: &'g Grid, pub expansions: u64, pub low_level_expansions: u64 }

impl<'g> Cbs<'g> {
    /// Sharon et al., Algorithm 1: best-first on SIC over a binary constraint tree.
    pub fn solve(&mut self, starts: &[GridIdx], goals: &[GridIdx]) -> Option<Vec<Vec<GridIdx>>> {
        let mut open = BinaryHeap::new();
        open.push(self.root(starts, goals)?);
        while let Some(n) = open.pop() {
            self.expansions += 1;
            let Some(c) = first_conflict(&n.paths) else { return Some(n.paths) };
            for (agent, constraint) in split(&c) {
                let mut cs = n.constraints.clone(); cs.push(constraint);
                // Re-plan only the constrained agent; inherit every other path verbatim.
                let own: Vec<_> = cs.iter().filter(|k| k.agent == agent).collect();
                if let Some(path) = self.low_level(starts[agent], goals[agent], &own) {
                    let mut paths = n.paths.clone(); paths[agent] = path;
                    let cost = sic(&paths, goals);
                    open.push(CtNode { constraints: cs, paths, cost });
                }
            }
        }
        None
    }
}

The manipulation graph and the propagation trait are the two remaining types; the first is a petgraph::UnGraph over (placement, grasp), the second is as small as the preview allows:

pub enum Mode { Transit { placement: usize }, Transfer { grasp: usize } }

pub struct Grasp { pub alpha: f64 }      // the puck's bearing in the hand frame
pub struct Placement { pub x: f64, pub y: f64 }

impl PuckArm {
    /// c_g(q): the held puck is a rigid transform of the hand.
    pub fn held_center(&self, q: &T2, g: &Grasp) -> Point2 {
        let tip = self.arm.tip(q); let h = self.arm.tip_heading(q) + g.alpha;
        Point2::new(tip.x + self.hold * h.cos(), tip.y + self.hold * h.sin())
    }
    /// Grasp configurations at (p, g): Chapter 2's IK on a virtual arm whose last link is
    /// link 2 extended by the hold offset, so that the virtual tip is the puck's center.
    pub fn grasp_configs(&self, p: &Placement, g: &Grasp) -> Vec<T2> {
        let w = Vector2::new(self.arm.l[1] + self.hold * g.alpha.cos(), self.hold * g.alpha.sin());
        let virt = self.arm.with_lengths([self.arm.l[0], w.norm()]);
        virt.ik(p.into()).into_iter().flatten().map(|(t1, t2)| T2::new(t1, t2 - w.y.atan2(w.x))).collect()
    }
}

/// MG: nodes (p, g, ik branch); edges transit (same p) or transfer (same g), each with a FuzzyPRM belief.
pub struct ManipulationGraph { pub g: UnGraph<MgNode, MgEdge>, pub start: NodeIndex, pub goals: Vec<NodeIndex> }

/// PREVIEW (Chapter 21 owns the full version). A simulator step; Hitch's Chapter 2 kinematic
/// update implements it as a black box. Nothing about the dynamics is derived or inspected here.
pub trait Propagate<M: Manifold> {
    type Control;
    fn propagate(&self, q: &M::Point, u: &Self::Control, dt: f64) -> M::Point;
}

impl Propagate<SE2> for Hitch {
    type Control = CarCmd;
    fn propagate(&self, q: &Pose2, u: &CarCmd, dt: f64) -> Pose2 { self.step(&HitchCfg { body: *q, trailer: None }, u, dt).body }
}

pub struct KinoRrtPreview<M: Chart, P: Propagate<M>> {
    pub tree: Tree<M>, pub sim: P, pub controls: Vec<Option<P::Control>>, pub k: usize, pub dt: f64, pub rng: SmallRng,
}

impl<M: Chart, P: Propagate<M>> KinoRrtPreview<M, P> {
    /// Sample q_rand; nearest; sample k controls; propagate each in substeps, checking every state;
    /// keep the free result closest to q_rand (LaValle & Kuffner's rule). No Steer, no Extend.
    pub fn extend_once(&mut self, free: &mut dyn FreeSpace<M>, sample_u: &mut dyn FnMut(&mut SmallRng) -> P::Control) -> Option<NodeId> {
        let q_rand = self.tree.space.sample(&mut self.rng);
        let (near, _) = self.tree.nearest(&q_rand);
        let mut best: Option<(P::Control, M::Point, f64)> = None;
        for _ in 0..self.k {
            let u = sample_u(&mut self.rng);
            let Some(q) = self.propagate_checked(free, &self.tree.q(near), &u) else { continue };
            let d = self.tree.space.dist(&q, &q_rand);
            if best.as_ref().map_or(true, |b| d < b.2) { best = Some((u, q, d)); }
        }
        let (u, q, _) = best?;
        let id = self.tree.add(q, near);
        self.controls.push(Some(u));
        Some(id)
    }
}

cargo run --example three_rustys -p multi runs prioritized planning in both orders, coupled PRM on Product<R2, 3> and CBS on the inflated Apartment grid, printing success, makespan, SIC and check counts as a snapshot test at seed 14; hitch_no_steer grows KinoRrtPreview in the Lot and asserts the goal region within 4 000 iterations. The TypeScript checks behind this page do the same: on seed 14 the forward-propagated tree reaches the Lot goal at iteration 63 with 63 nodes after 504 propagations, 140 of them wasted on a collision; the path has 14 nodes, and re-running its 13 stored controls through Hitch.step reproduces every one of them to the last bit, all 66 simulator states are free, and the chord-versus-mean-heading mismatch of every edge is zero to machine precision — the signature of an exact arc.

Putting it together: three ways to pay

The integration lab is the three widgets above read side by side, with the bills totted up.

Three Rustys. In the swap corridor, prioritized planning fails in both orders at a cost of about 3 000 configuration–time RRT iterations each; the decoupled coordinator fails in one grid search over a $48 \times 48$ diagram; CBS on the grid version succeeds with a sum of costs of 15 after 12 constraint-tree expansions and 23 nodes, each expansion two time-expanded A* searches; the brute-force joint A* agrees on 15 after 19 expansions and 138 joint successors — cheap for two agents, and on the three-agent room already 1412 successors against CBS's three nodes. The coupled RRT-Connect in $(\mathbb R^2)^2$ succeeds at 2562 attempts and 3278 composite checks. Every one of those numbers is produced by the code behind the page and pinned by a check.

Reach and the puck. Seven edges in the manipulation graph, four verified, 218 collision checks at the lower level, one upper-level round. Switch the eager toggle in w14.2 and every edge is planned whether or not it is needed — Exercise 4 asks you to count.

Hitch without a steer. The preview above. One control per extension and the tree wanders; twenty and most propagations are thrown away; eight is the default because that is where, on the seeds tried, the time to the goal region stopped improving. The grey ghost is Chapter 12's RRT on SE(2) pretending $\theta$ is a coordinate like any other: it reaches the goal too, along a path that slides sideways by several radians in total. A car cannot execute it. The blue tree's path it can, because every edge was an execution. This is as far as the chapter goes: the simulator is a black box, and Chapter 21 opens it.

OMPL and MoveIt

Everything in Part III is infrastructure in two systems you will meet in practice. The Open Motion Planning Library (Şucan, Moll and Kavraki 2012) is the sampling-based core; MoveIt (Coleman, Şucan, Chitta and Correll 2014) wraps it with a planning scene, kinematics plugins and request adapters for manipulators. The mapping onto this book's traits is interface-level and worth having in one place:

The point of the table is not that the names line up; it is that nothing in those systems is a different kind of thing from what this book has built, body by body. The capstone's stack is mapped onto OMPL, MoveIt and Nav2 through this table in Chapter 23.

Where §7.5.4–7.5.6 went

Choset's remaining three subsections — assembly planning, flexible objects, biological applications — are each a configuration space with a different shape under the same machinery: one subassembly as the robot and the rest as the workspace; a deformable body with an energy threshold standing in for a free-space predicate; a molecule as a tree-like articulated robot with dihedral angles as joints. The pointer box above condenses each to a paragraph. Nonprehensile manipulation — parts feeding, squeeze grasps, programmable vector fields, the $2n - 1$ squeezes that orient any polygonal part — and the NonDirectional Blocking Graph are mentioned in those paragraphs and taught nowhere in this book. Parallel SRT, which Choset notes plans for many robots by prioritizing each roadmap edge and centralizing its endpoints, is a sentence here and an exercise in Chapter 12.

Part IV begins with the assumption every chapter so far has made in silence: that the robot knows where it is.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Which C-obstacle dominates

   Two unit-radius discs translate in $[0, 10]^2$ each, so $\Q = [0, 10]^4$. Compute the four-dimensional measure of $\QO_{12} \cap [0,10]^4$ exactly (hint: it is the measure of the set of pairs of centers within distance 2, which is $\int\!\!\int$ of the area of a disc of radius 2 clipped to the square — bound it above and below first), and compare it with the measure of $\QO_1^{\W}$ for a $1 \times 1$ pillar, lifted to $\Q$. Then let the number of discs grow to $m$ and say which family of obstacles dominates the measure of $\Q \setminus \Qfree$, and why the answer is the same statement as "pair checks grow like $m^2$."

   Ignoring the clipping at the square's edges, what fraction of [0,10]⁴ does 𝒬𝒪₁₂ occupy? (Area of a disc of radius 2 over the area of the square.)

   Check Reveal
2. Foundation exerciseFDifficulty 3 of 3CBS never prunes a solution

   Prove Step 5 of the CBS derivation with edge conflicts included: for an edge conflict in which $a_i$ traverses $u \to v$ and $a_j$ traverses $v \to u$ between $t - 1$ and $t$, show that every conflict-free solution consistent with the parent is consistent with at least one of the two children. Then construct a two-agent instance whose solution lies at depth $\ge 3$ in the constraint tree — three constraints before the first conflict-free node — and verify your depth with the Cbs class's solve().depth.

   Stuck? Get a hint
3. Conceptual exerciseCDifficulty 1 of 3Predict the width at which both orders fail
   Predict first

   In w14.1 at 3.2 radii of corridor width both prioritized orders fail. Widen the corridor. Before dragging: at what width do both orders start to succeed, and what happens to the coordination diagram's band there?

   At exactly 4 radii; the band disappears entirely

   Somewhat above 4 radii; the band stays as long as both paths are the axis

   Never: the alcove is the only passing place
4. Conceptual exerciseCDifficulty 2 of 3Minimum grasps, and the price of eagerness
   Predict first

   In w14.2 with |𝒢| = 1 the manipulation graph is disconnected. What is the minimum |𝒢| that connects it with placement C enabled, and through which node does the path pass?

   |𝒢| = 2, regrasping at C from the front grasp to the left grasp

   |𝒢| = 2, a single transfer from A to B

   |𝒢| = 3 — the right grasp is needed too
5. Practical exercisePDifficulty 2 of 3One robot at a time

   Implement Choset's one-robot-at-a-time extension (§7.5.2, after [C ref. 14]) as an Extend implementation on Product<R2, N>: move robot 1 toward its target and check it against the obstacles; then robot 2, checked against the obstacles and robot 1's new position; and so on, re-sampling a robot's target when it collides. Compare its coupled-RRT success rate at a fixed budget with the componentwise extend of Chapter 12 over 50 seeds in a four-Rusty Apartment, and report the mean number of configurations "close to obstacles" that Choset says the plain extension produces.

6. Practical exercisePDifficulty 3 of 3Makespan CBS against coupled A*

   Add a makespan objective to Cbs — the high level keyed on $\max_i \mathrm{cost}_i$ rather than the sum — keeping edge conflicts, and prove or refute that the first conflict-free node popped is still optimal (the sum argument used integrality and monotonicity; check both). Benchmark CBS under both objectives against jointAStar on the inflated Apartment grid with 3 to 6 agents, plotting constraint-tree expansions and joint successors generated against the number of agents, and mark the agent count at which the joint search stops finishing.

References

1. Sharon, G., Stern, R., Felner, A., and Sturtevant, N. R. (2015) Conflict-Based Search for Optimal Multi-Agent Pathfinding. Artificial Intelligence 219, 40–66.doi:10.1016/j.artint.2014.11.006 (opens in a new tab)

   CBS as this chapter states and proves it: the two-level search, vertex and edge conflicts, the constraint tree, and the optimality argument of Derivation 3.

2. Choset, H., Lynch, K. M., Hutchinson, S., Kantor, G., Burgard, W., Kavraki, L. E., and Thrun, S. (2005) Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press.link to Principles of Robot Motion: Theory, Algorithms, and Implementations (opens in a new tab)

   §7.5: control-based planning, centralized/decoupled/prioritized multi-robot planning with its incompleteness remark and the one-robot-at-a-time extension [ref. 14], transit/transfer manipulation with the two-level FuzzyPRM of Nielsen and Kavraki [ref. 335] and aSyMov [ref. 170], and the velocity coordination of Kant and Zucker [ref. 219]; the epigraph is from §7.5.2.

3. LaValle, S. M. and Kuffner, J. J. (2001) Randomized Kinodynamic Planning. International Journal of Robotics Research 20(5), 378–400.doi:10.1177/02783640122067453 (opens in a new tab)

   The forward-propagated RRT of the preview — sample controls, integrate, keep the result closest to the random state — in its original kinodynamic setting; Chapter 21 returns to it in full.

4. Kuffner, J. J. and LaValle, S. M. (2000) RRT-Connect: An Efficient Approach to Single-Query Path Planning. IEEE International Conference on Robotics and Automation.doi:10.1109/ROBOT.2000.844730 (opens in a new tab)

   The bidirectional planner the coupled lab and the manipulation planner's lower level run unchanged on the composite space and on each mode's torus.

5. Kavraki, L. E., Švestka, P., Latombe, J.-C., and Overmars, M. H. (1996) Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces. IEEE Transactions on Robotics and Automation 12(4), 566–580.doi:10.1109/70.508439 (opens in a new tab)

   The roadmap whose two-level, lazily verified descendant plans the manipulation graph's edges, and whose sample-count bound Derivation 1 reads with d = Σ dᵢ.

6. Garrett, C. R., Chitnis, R., Holladay, R., Kim, B., Silver, T., Kaelbling, L. P., and Lozano-Pérez, T. (2021) Integrated Task and Motion Planning. Annual Review of Control, Robotics, and Autonomous Systems 4, 265–293.link to Integrated Task and Motion Planning (opens in a new tab)

   The task-and-motion view that generalizes the manipulation graph: symbolic actions as modes, continuous parameters as grasps and placements, interleaved discrete and continuous search.

7. Şucan, I. A., Moll, M., and Kavraki, L. E. (2012) The Open Motion Planning Library. IEEE Robotics and Automation Magazine 19(4), 72–82.doi:10.1109/MRA.2012.2205651 (opens in a new tab)

   The system in which Part III is infrastructure; the interface mapping StateSpace ↔ Manifold, StateSampler ↔ Sampler, MotionValidator ↔ Steer, Planner ↔ Prm/Rrt/RrtStar is drawn from its design.

8. Coleman, D., Şucan, I. A., Chitta, S., and Correll, N. (2014) Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study. Journal of Software Engineering for Robotics 5(1).link to Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study (opens in a new tab)

   MoveIt as the planning scene, kinematics plugins and request adapters around OMPL — the manipulation-side half of the systems table.