Math 6 · Motion-planning mathematics

Why this chapter exists. OMPL's code is simple to read: sample, find nearest, steer, check, repeat. Its design decisions, though, all come from theory: why sample at all, why uniform on SO(3) needs care, why the collision-check resolution matters, why RRT*'s neighbourhood shrinks like \((\log n/n)^{1/d}\), why informed sampling is an ellipse. This chapter builds that theory from the definition of configuration space up, and points at the OMPL code that implements each piece. It depends on chapters 1 and 2 only.

1. The planning problem, formally

• Workspace \(\mathcal W\subset\R^2\) or \(\R^3\), with obstacles \(\mathcal O\subset\mathcal W\).
• Robot \(\mathcal A(q)\subset\mathcal W\): the set of points occupied by the robot at configuration \(q\).
• Configuration space (C-space) \(\mathcal C\): the set of all configurations, i.e. every independent number needed to place every point of the robot.
• C-obstacle \(\mathcal C_{obs} = \{q\in\mathcal C:\mathcal A(q)\cap\mathcal O\ne\emptyset\}\), and free space \(\mathcal C_{free} = \mathcal C\setminus\mathcal C_{obs}\) (plus joint limits, self-collision, etc.).
• Feasibility problem: given start \(q_I\) and goal region \(\mathcal G\subseteq\mathcal C_{free}\), find a continuous \(\sigma:[0,1]\to\mathcal C_{free}\) with \(\sigma(0)=q_I\) and \(\sigma(1)\in\mathcal G\), or report that none exists.
• Optimal planning: among feasible paths, minimize a cost functional \(c(\sigma)\) (length, clearance, energy, …).

OMPL maps these one-to-one: StateSpace is \(\mathcal C\), StateValidityChecker::isValid is the membership test for \(\mathcal C_{free}\), ProblemDefinition holds \((q_I,\mathcal G,c)\), and PathGeometric is \(\sigma\) (as a polyline in \(\mathcal C\)).

2. What C-space looks like

Topology matters. The shortest path between headings \(179°\) and \(-179°\) is \(2°\), not \(358°\). A planner that treats SO(2) as an interval plans the long way round. OMPL's spaces encode topology in distance, interpolate, and enforceBounds (chapter 2 §17).

3. C-obstacles, and why we can't compute them

For a robot that only translates, the C-obstacle is a Minkowski sum: \(\mathcal C_{obs} = \mathcal O\oplus(-\mathcal A(0)) = \{o - a: o\in\mathcal O, a\in\mathcal A(0)\}\). In other words, grow the obstacles by the reflected robot shape and shrink the robot to a point. For convex polygons this is easy, with \(O(m+n)\) edges.

Once the robot rotates or has joints, \(\mathcal C_{obs}\) becomes a curved, high-dimensional, implicitly defined set. Each C-obstacle's boundary is described by polynomial inequalities in \(\cos q_i,\sin q_i\). The general piano-mover's problem is PSPACE-hard (Reif 1979). Canny's roadmap algorithm (1988) is complete but singly exponential in the dimension, which is impractical beyond about 3–4 DoF.

Sampling-based planning sidesteps the problem completely. It never represents \(\mathcal C_{obs}\), and only asks an oracle "is this \(q\) free?" (a collision checker in the workspace, e.g. FCL). OMPL is built around that oracle.

4. What "complete" can mean

• Complete: finds a solution if one exists and reports failure otherwise, in finite time.
• Resolution complete: complete at a given discretization resolution (grid search, deterministic sampling).
• Probabilistically complete: \(\lim_{n\to\infty}\Pr[\text{solution found after }n\text{ samples}] = 1\) whenever a solution exists. It cannot prove that no solution exists.

The difficulty of a problem is captured by clearance. A path \(\sigma\) has clearance \(\delta\) if the ball \(B_\delta(\sigma(t))\subseteq\mathcal C_{free}\) for all \(t\). Small clearance means a narrow passage: a region of tiny volume that random samples rarely hit. All the convergence rates below get worse as clearance shrinks.

5. Metrics on C-space

Planners need a distance \(\rho(q,q')\) that is a metric: non-negative, zero only for equal states, symmetric, and satisfying the triangle inequality. It drives three things: which tree node is "nearest", how far a single steering step goes, and the RRT*/PRM* connection radii. Common choices:

• \(\R^n\): Euclidean. Joint spaces sometimes use per-joint weights, e.g. by downstream link length.
• SO(2): the shorter arc, \(\min(\lvert a-b\rvert, 2\pi-\lvert a-b\rvert)\).
• SO(3): the angle \(\theta = 2\arccos\lvert q_1\cdot q_2\rvert\). OMPL uses \(\arccos\lvert q_1\cdot q_2\rvert = \theta/2\), a constant multiple, so it is still a metric, with max extent \(\pi/2\). An alternative is the chordal \(\lVert R_1-R_2\rVert_F = 2\sqrt2\sin(\theta/2)\).
• SE(3): OMPL uses the weighted sum \(w_t\lVert\Delta t\rVert + w_r\,d_{SO3}\), with both weights 1 by default. Metres and radians don't share units. A sensible choice is to make both terms approximate how far the robot's surface moves. Rotating a body of radius \(r_b\) by \(\theta\) moves its farthest point about \(r_b\theta = 2r_b\cdot d_{SO3}\), so set \(w_r\approx2r_b\) relative to \(w_t=1\) (CompoundStateSpace::setSubspaceWeight).

A metric that doesn't reflect actual motion makes nearest-neighbour choices poor and collision-check resolution uneven (§9).

6. Volume, and uniform sampling on manifolds

Volume. \(\mu(\cdot)\) is the Lebesgue measure (volume). The unit \(d\)-ball has volume \(\zeta_d = \pi^{d/2}/\Gamma(\tfrac d2+1)\): \(\zeta_2=\pi\), \(\zeta_3=\tfrac43\pi\). OMPL reports a space's volume with getMeasure(): the product of side lengths for bounded \(\R^n\), \(2\pi\) for SO(2), and \(\pi^2\) for SO(3) (half of \(S^3\)'s \(2\pi^2\), because of the double cover). RRT*'s radius formula (§12) uses it.

Uniform means "no preferred region": \(\Pr[q\in U] = \mu(U)/\mu(\mathcal C)\). On boxes, sample each coordinate uniformly. On SO(3), "uniform" means the Haar measure, the unique distribution invariant under rotating all samples by any fixed rotation. Under it, no orientation is special.

Euler angles are not uniform. Sampling roll, pitch, and yaw uniformly over-samples orientations near pitch \(\pm90°\). The volume element in Euler coordinates is \(\cos(\text{pitch})\,d\phi\,d\vartheta\,d\psi\), so the poles are crowded with samples that represent a small volume.

Uniform on SO(3) = uniform unit quaternion. Because \(S^3\to SO(3)\) is a 2-to-1 covering that respects the group structure, the uniform distribution on \(S^3\) maps to Haar measure on SO(3). There are two ways to sample it:

1. Draw 4 independent standard normals and normalize. This works because the Gaussian is rotationally symmetric.
2. Shoemake's method (used by OMPL's RNG::quaternion): draw \(u_1,u_2,u_3\sim U(0,1)\) and set

Why it works: view \(S^3\subset\mathbb C^2\) as \((z_1,z_2)\) with \(\lvert z_1\rvert^2+\lvert z_2\rvert^2=1\). For a uniform point on \(S^3\), \(\lvert z_2\rvert^2\) is uniform on \([0,1]\) (an Archimedes-style fact for \(S^3\)), and the two phases are independent and uniform. That's exactly \(u_1\), \(2\pi u_2\), and \(2\pi u_3\).

Uniform in a \(d\)-ball (RNG::uniformInBall): take a direction from normalized Gaussians and a radius \(r\,u^{1/d}\). The \(1/d\) power is needed because \(\Pr[\lVert x\rVert\le s] = (s/r)^d\); inverting the CDF gives it. Without it, samples cluster at the centre.

7. Dispersion, low-discrepancy sequences, and the curse of dimensionality

Dispersion \(\delta(P) = \sup_{q\in\mathcal C}\min_{p\in P}\rho(q,p)\) is the radius of the largest empty ball. A planner can't "see" passages narrower than about the dispersion.

• A grid with \(n\) points in \([0,1]^d\) has dispersion \(\Theta(n^{-1/d})\).
• \(n\) uniform random samples have dispersion \(O\big((\log n/n)^{1/d}\big)\) with high probability. That's almost as good, with no structure needed.
• The curse of dimensionality. Halving the dispersion needs \(2^d\) times more samples: 128× in a 7-DoF arm space. That's why sampling-based planning works in high dimensions only because real free space is usually not maze-like everywhere.

Low-discrepancy sequences fill space more evenly than random samples. The Halton sequence uses the radical inverse in a different prime base per dimension: write \(i\) in base \(p\) and mirror the digits after the radix point. In base 2 this gives \(\tfrac12,\tfrac14,\tfrac34,\tfrac18,\tfrac58,\dots\) and in base 3 \(\tfrac13,\tfrac23,\tfrac19,\tfrac49,\dots\). The result is deterministic and reproducible, and makes planners resolution complete. OMPL: base/samplers/deterministic/HaltonSequence.h, DeterministicStateSampler.

8. Sampling for narrow passages

Uniform sampling rarely lands in narrow passages, so OMPL ships biased valid-state samplers (base/samplers/):

9. Local planning and collision-check resolution

A planner connects \(q\) to \(q'\) along the space's interpolate (straight line, slerp, or a Dubins curve) and must certify the whole segment free. With only a point-membership oracle, it checks discrete points along the way. OMPL's DiscreteMotionValidator checks

What it guarantees. Suppose no point of the robot moves more than \(L\cdot\rho(q,q')\) in the workspace when the configuration moves by \(\rho(q,q')\) (\(L\) is a Lipschitz constant of the forward kinematics in your metric). Every configuration on the segment is within \(\epsilon/2\) of a checked one, so its robot points are within \(L\epsilon/2\) of a checked placement. If every checked configuration has workspace clearance \(> L\epsilon/2\), the whole segment is collision-free. Conversely, obstacles thinner than about \(L\epsilon\) can slip between checks. For a single 1 m link with a \(2\pi\) joint range, the 1% default gives \(\epsilon\approx0.063\) rad and \(L\epsilon\approx6\) cm. A 7-joint arm with similar ranges has maximum extent \(\approx\sqrt7\cdot2\pi\approx16.6\) rad, so \(\epsilon\approx0.17\) rad and \(L\epsilon\) can reach 10–20 cm. Set the fraction from your robot's size and the thinnest obstacle you care about.

Check order. DiscreteMotionValidator checks the endpoint first, then midpoints recursively (bisection, van der Corput order). Collisions tend to be spread out along an edge, so checking the middle early finds them with fewer checks on average than a front-to-back sweep. The final verdict is the same either way.

Exact alternatives: continuous collision detection (swept volumes) or conservative advancement with distance queries. These are available in FCL, and VAMP exploits SIMD to check many states of an edge at once.

10. Nearest-neighbour search

Each RRT iteration asks "which tree node is nearest to \(q_{rand}\)?". Brute force is \(O(n)\) per query, \(O(n^2)\) for the whole run.

• k-d trees: excellent in low-dimensional Euclidean spaces. They assume axis-aligned splits and so break with wrap-around (SO(2)), quaternions, and Dubins distances. They degrade toward linear scan in high dimensions.
• Metric trees work for any metric because they rely only on the triangle inequality. GNAT (Brin 1995, OMPL's default NearestNeighborsGNAT): each node picks \(k\) pivots (default degree 8, between 4 and 12). Every point goes to the child of its nearest pivot. The node stores, for every pivot \(p_i\) and child \(j\), the range \([\ell_{ij},u_{ij}]\) of distances from \(p_i\) to the points in child \(j\). Pruning: for a query \(q\) with current search radius \(r\), if \(\rho(q,p_i)-r>u_{ij}\) or \(\rho(q,p_i)+r<\ell_{ij}\), then by the triangle inequality no point of child \(j\) can be within \(r\) of \(q\), so the whole child is skipped.
• Approximate NN (NearestNeighborsSqrtApprox, FLANN) trades exactness for speed. Planners stay probabilistically complete with approximate NN, but optimality guarantees weaken.

NearestNeighborsLinear (brute force) is the oracle to test any port against.

11. RRT and PRM: why they work

11.1 RRT and the Voronoi bias

RRT (geometric/planners/rrt/RRT.cpp) repeatedly samples \(q_{rand}\), finds the nearest node \(q_{near}\), steers a step of at most range toward \(q_{rand}\), and adds the new node if the motion is valid. Goal bias defaults to 0.05; range defaults to \(0.2\times\) maximum extent.

Why it explores fast: node \(v\) is selected for extension exactly when \(q_{rand}\) falls in \(v\)'s Voronoi cell, the set of points closer to \(v\) than to any other node. So \(\Pr[\text{extend }v] = \mu(\text{Vor}(v))/\mu(\mathcal C)\). Nodes on the frontier of the tree have huge Voronoi cells, since they border unexplored space, so the tree is pulled outward into unexplored regions. Interior nodes have small cells and are rarely extended.

Probabilistic completeness (sketch). Take a solution path with clearance \(\delta\) and cover it with a sequence of balls of radius \(\delta/2\) spaced less than the step size apart. If the tree has a node in ball \(k\), then any sample landing in ball \(k+1\) produces a valid extension into it, because the straight segment stays within the \(\delta\)-tube. Each iteration does this with probability at least \(p = \mu(B_{\delta/2})/\mu(\mathcal C)>0\). Reaching the last ball needs a fixed number \(M\) of such successes, so the failure probability after \(n\) iterations decays exponentially in \(n\). (The original LaValle–Kuffner argument had gaps; Kleinbort et al. 2019 gave a rigorous proof.)

RRT-Connect grows trees from the start and the goal, alternately extending one tree and greedily connecting the other (repeated extend until blocked). It needs reversible (symmetric) steering and a sampleable goal. It is often orders of magnitude faster, and is OMPL's default geometric planner.

11.2 PRM and its failure bound

PRM (geometric/planners/prm/PRM.cpp) samples \(n\) free configurations, tries to connect each to its \(k\) nearest (default \(k=10\)) with the local planner, and answers queries by connecting start and goal to the roadmap and running graph search (A*). It is multi-query: build once, query many times.

Failure bound (Kavraki, Kolountzakis, Latombe 1998). For a solution path of length \(L\) and clearance \(\delta\), cover it with \(\lceil2L/\delta\rceil\) balls of radius \(\delta/2\). If every ball contains a sample, consecutive samples can be connected. A union bound over the balls gives

The failure probability decays exponentially in \(n\), with a rate set by clearance relative to volume: narrow passages (\(\delta\) small) and high dimension (\(\mu(B_{\delta/2})\propto\delta^d\)) make it slow.

Lazy PRM builds the graph without checking edges, searches for a shortest path, checks only that path's edges, deletes invalid ones, and repeats. Collision checks are spent only where they matter.

12. Asymptotic optimality, and where the (log n / n)^(1/d) radius comes from

RRT is not optimal. Each node's parent is fixed at insertion and never revisited. Karaman & Frazzoli (2011) showed RRT's solution cost converges, with probability 1, to a suboptimal value.

Random geometric graphs. Place \(n\) uniform points in a region of volume \(\mu\) and connect pairs within distance \(r\). The expected number of neighbours of a point is

For the graph to stay connected as \(n\) grows, no point may be isolated. That needs \(\bar k\) to grow like \(\log n\), a coupon-collector-style threshold (Penrose 1997). Solving \(n\zeta_dr^d/\mu = \gamma'\log n\) gives

This is the slowest-shrinking-without-wasting-work radius. A larger radius costs more collision checks per sample, and a smaller one disconnects the graph and loses optimality.

PRM* connects within \(r(n)\) with \(\gamma>\gamma^\star = 2(1+\tfrac1d)^{1/d}\big(\tfrac{\mu(\mathcal C_{free})}{\zeta_d}\big)^{1/d}\), or to the \(k(n) = \lceil k_{PRM}\log n\rceil\) nearest with \(k_{PRM}>e(1+\tfrac1d)\). Both are asymptotically optimal: the best path cost in the roadmap converges to the optimum. OMPL's PRMstar uses the \(k\)-nearest form with exactly \(k = \lceil e(1+1/d)\log n\rceil\) (KStarStrategy).

RRT* adds two steps to RRT. For each new node \(q_{new}\), with neighbourhood \(N\) = the \(k(n)\)-nearest or the nodes within \(r(n)\):

1. Choose parent: connect \(q_{new}\) from the neighbour minimizing cost-to-come \(c(q) + c(q, q_{new})\) (if collision-free).
2. Rewire: for each neighbour \(q\in N\), if going through \(q_{new}\) is cheaper, \(c(q_{new}) + c(q_{new},q) < c(q)\), make \(q_{new}\) its parent (and propagate the cost change to its descendants).

OMPL's constants (RRTstar::calculateRewiringLowerBounds):

with rewire_factor \(f_{rw}=1.1\). \(\mu\) is the space measure, or the informed subset's measure when pruning (§13). The \(k\) constant is more conservative than the paper's \(e(1+1/d)\). The neighbourhood at iteration \(n\) is \(\lceil k_{rrt}\log(n+1)\rceil\) nodes, or radius \(\min(\texttt{range}, r_{rrt}(\log(n+1)/(n+1))^{1/d})\). Per-iteration cost stays \(O(\log n)\), the same order as RRT, so optimality is nearly free asymptotically.

13. Informed sampling: the ellipse that shrinks

Once a solution of length \(c_{best}\) exists, only states \(q\) with

can be on a better path, since any path through \(q\) is at least that long by the triangle inequality. This set is a prolate hyperspheroid: an ellipsoid with foci \(q_{start}\) and \(q_{goal}\), transverse diameter \(c_{best}\), and all other diameters \(\sqrt{c_{best}^2 - c_{min}^2}\), where \(c_{min} = \lVert q_{goal}-q_{start}\rVert\).

Direct sampling (Gammell et al. 2014; OMPL PathLengthDirectInfSampler, util/ProlateHyperspheroid):

with \(x_{ball}\) uniform in the unit \(d\)-ball (§6) and \(C\) the rotation taking the first axis to \((q_{goal}-q_{start})/c_{min}\) (from an SVD, with the determinant fixed to \(+1\) as in chapter 1 §11). The volume is \(\zeta_d\frac{c_{best}}{2}\big(\frac{\sqrt{c_{best}^2-c_{min}^2}}{2}\big)^{d-1}\). It shrinks as the solution improves, so sampling focuses exactly where improvement is possible, and convergence no longer slows with the size of the original space.

This direct form is exact only for path length in Euclidean (sub)spaces. For other objectives OMPL falls back to rejection sampling against a cost heuristic (RejectionInfSampler).

14. Graph search inside the planners

• Dijkstra: expand nodes in order of cost-to-come \(g\) using a priority queue. When a node is popped its \(g\) is final, because all edges are non-negative.
• A*: order by \(f = g + h\). If \(h\) never overestimates the true cost-to-go (admissible), the first goal popped is optimal. If \(h(u)\le c(u,v)+h(v)\) (consistent), no node is ever reopened. Euclidean distance is a consistent heuristic for path length. PRM queries use Boost.Graph's astar_search.
• Incremental search (LPA*, D* Lite): when edge costs change (e.g. an edge found to be in collision), repair the previous search instead of restarting.
• BIT* treats batches of samples as an implicit random geometric graph (§12) and searches it A*-style, ordering candidate edges by \(\hat g(v)+\hat c(v,x)+\hat h(x)\) computed from cheap heuristics. It runs the collision check only when an edge is about to be added (lazy). AIT* computes a better, problem-specific heuristic with a lazy reverse search, and EIT* also estimates collision-checking effort. They are among the strongest planners in OMPL for optimal planning (geometric/planners/informedtrees).
• FMT*: dynamic programming over a fixed batch with \(r(n)\) connections. It expands the lowest-cost open node and connects unvisited neighbours via their locally best parent, checking only that one edge. It is asymptotically optimal with the PRM* radius.

15. Costs as an algebra

OMPL's OptimizationObjective treats cost as an abstract algebra, so one planner implementation can serve many objectives. It needs:

• combineCosts(a, b): how costs accumulate along a path (\(+\) for length; \(\max\) or \(\min\) for minimax/clearance),
• identityCost, infiniteCost,
• isCostBetterThan(a, b) (\(<\) for length; reversed for maximizing clearance),
• motionCost(s1, s2) and stateCost(s).

Concrete objectives:

• Path length: motionCost is \(\rho(s_1,s_2)\).
• State-cost integral: the trapezoid rule \(\tfrac12(c(s_1)+c(s_2))\rho(s_1,s_2)\).
• Mechanical work (T-RRT): only cost increases are penalized, plus a small length term.

Monotonicity requirement. RRT*-style rewiring assumes that extending a path never makes it better, i.e. combining with a non-negative motion cost doesn't decrease cost. Costs that violate this break the optimality arguments in §12.

16. Kinodynamic planning

Dynamics. The state \(x\) (e.g. pose + velocities) evolves by \(\dot x = f(x,u)\) with controls \(u\in\mathcal U\). Paths must be trajectories that some control input can produce. Example, the kinematic car (rear axle, wheelbase \(L\), speed \(v\), steering \(\phi\)):

This is nonholonomic: the car can't move sideways, yet it can still reach any pose by manoeuvring.

The steering problem. Connecting two given states exactly is a two-point boundary-value problem, generally as hard as optimal control. Two ways out:

1. Analytic steering for special systems. Dubins car (forward only, minimum turning radius \(\rho\)): the shortest path between two poses is one of 6 words, LSL, RSR, LSR, RSL, RLR, LRL (L/R = turn at full lock, S = straight). Dubins proved this in 1957; Pontryagin's maximum principle gives a modern proof. Reeds–Shepp (forward and reverse): a finite set of 48 path types (1990). OMPL's DubinsStateSpace and ReedsSheppStateSpace use these as distance/interpolate, so geometric planners plan for cars directly. Dubins distance is not symmetric, which matters for bidirectional planners.
2. Forward propagation. Sample a control and duration, integrate the dynamics, and keep the result. OMPL's control planners (control::RRT, KPIECE1, EST, PDST, SST) all work this way, needing only StatePropagator::propagate. Durations are multiples of the propagation step size, between the configured minimum and maximum control durations.

Numerical integration (ODESolver, via Boost.Odeint). Euler, \(x_{k+1}=x_k+hf(x_k,u)\), has global error \(O(h)\). RK4 has global error \(O(h^4)\) (local \(O(h^5)\)):

The adaptive variant (Cash–Karp 5(4)) estimates its own error from two embedded orders and adjusts the step size. After integrating, call enforceBounds so quaternions are renormalized and angles wrapped. OMPL asserts this in SO3StateSpace::distance.

Guarantees. Forward-propagating RRT is probabilistically complete under mild conditions (Kleinbort et al. 2019). SST (Li, Littlefield & Bekris 2016) keeps only the best node per small "witness" region and is asymptotically near-optimal, with a sparse data structure.

Projections (KPIECE, EST, PDST). Without good steering, nearest-neighbour expansion is unreliable. Instead these planners project states to a low-dimensional space \(E:\mathcal X\to\R^k\) (a ProjectionEvaluator, e.g. the \(x,y\) of the base), lay a grid over it, and expand from sparsely covered, boundary ("exterior") cells. Coverage estimates replace distances. That is why SelfConfig picks KPIECE1 for control problems whose space has a default projection.

17. Constrained planning: paths on implicit manifolds

Task constraints are equations \(F(q)=0\), \(F:\R^n\to\R^m\): keep a tray level, keep a gripper on a door handle, close a kinematic loop. If the Jacobian \(J_F\) has full row rank, the valid set \(\mathcal M = \{q:F(q)=0\}\) is an \((n-m)\)-dimensional manifold with tangent space \(T_q\mathcal M = \operatorname{null}(J_F(q))\). Uniform sampling in \(\R^n\) hits \(\mathcal M\) with probability zero, so planners must work on \(\mathcal M\).

Projection (ProjectedStateSpace; OMPL Constraint::project): Newton's method with the minimum-norm step,

where OMPL computes \(J^+F\) with an SVD least-squares solve. Sampling is "sample in ambient space, then project". Interpolation is "step a little in the ambient direction, project, repeat", tracing a discrete geodesic along \(\mathcal M\).

Atlas (AtlasStateSpace): cover \(\mathcal M\) with charts. Each chart at a point \(c\) has an orthonormal tangent basis \(\Phi_c\) (\(n\times(n-m)\) with \(J_F\Phi_c=0\), from QR/SVD) and maps \(u\in\R^{n-m}\) to \(\psi_c(u) = \text{project}(c+\Phi_cu)\). Charts are created lazily as the planner explores, and their validity regions are bounded by parameters ε (distance from the manifold), ρ (chart radius), and α (angle to the tangent space). Sampling and interpolation then happen in low-dimensional chart coordinates. TangentBundleStateSpace is a lazier variant. Once \(\mathcal M\) is wrapped as a StateSpace, any geometric planner works unchanged.

This is the same mathematics as GTSAM's constrained optimization (gtsam/constrained): the null space of the constraint Jacobian, Newton projection, and charts on manifolds (chapter 2 §15).

18. Post-processing paths

• Shortcutting (PathSimplifier::reduceVertices, shortcutPath): pick two points on the path. If the direct local path between them is valid, replace the section between them. By the triangle inequality this never increases path length. Repeated random shortcutting quickly removes the jaggedness of sampling-based paths.
• Collapse close vertices: remove vertices closer than a threshold that aren't needed for validity.
• B-spline smoothing (smoothBSpline): repeatedly replace vertices with averages of their neighbours (Chaikin-like corner cutting), re-checking validity. This yields smooth curves suitable for execution.
• Perturbation (perturbPath): randomly perturb vertices and keep perturbations that lower a general cost (for objectives other than length).
• Interpolation for execution (PathGeometric::interpolate): densify to a fixed resolution before sending to a controller.

19. Summary of guarantees

20. Check yourself

1. Why does sampling Euler angles uniformly over-sample near pitch ±90°? Write the volume element.
2. Derive the radius of a uniform sample in a \(d\)-ball from its CDF.
3. With OMPL's SO(3) metric and a robot of radius 0.5 m, what rotation weight makes the SE(3) metric approximate surface displacement?
4. Explain the Voronoi bias of RRT in one paragraph without equations.
5. Derive \(r(n)\propto(\log n/n)^{1/d}\) from "expected number of neighbours ∝ \(\log n\)".
6. Why must RRT*'s cost combination be monotone? Construct a counterexample cost.
7. Why can't you just sample uniformly in ambient space for a constrained problem?