OMPL deep dive

This page goes from the idea to the code. Source: ompl/ompl main @ 5b209a0, version 2.0.2 (ABI 19), C++17.

0. The one-paragraph version

The robot's configuration space \(\mathcal{C}\) is a manifold such as \(\mathbb{R}^n\), \(SE(3)\), or products of these. A black-box predicate splits it into \(\mathcal{C}_{free}\) and \(\mathcal{C}_{obs}\). Given a start \(q_s\) and a goal region \(G\), find a continuous path \(\sigma:[0,1]\to\mathcal{C}_{free}\) with \(\sigma(0)=q_s\) and \(\sigma(1)\in G\), optionally minimizing a cost \(c(\sigma)\). Computing \(\mathcal{C}_{obs}\) explicitly is exponential in dimension, so OMPL never does it. Instead it samples configurations, checks them with the predicate, connects nearby valid samples with local straight-line motions (also checked), and grows a tree or roadmap until start and goal are connected. Everything in OMPL is one of four things: describing the space, describing the problem, a strategy for sampling and connecting (a planner), or post-processing and benchmarking the result.

1. Architecture at a glance

           user code / MoveIt / Python
                       │
              geometric::SimpleSetup  ──── tools::Benchmark, SelfConfig
                       │
      ┌────────────────┼─────────────────────┐
      ▼                ▼                     ▼
 base::Planner   base::ProblemDefinition   base::SpaceInformation
 (RRT, PRM, ...)  start states, Goal,       ├─ StateSpace (+ samplers, projections)
                  OptimizationObjective     ├─ StateValidityChecker   ← YOUR collision code
                                            └─ MotionValidator (discrete subdivision)

2. The problem description layer (ompl/base)

2.1 StateSpace and State: describing the manifold

A State is an opaque, empty base struct. Each space defines its own StateType subclass (RealVectorStateSpace::StateType { double *values; }, SO3StateSpace::StateType { double x,y,z,w; }) and casts with state->as<T>(). CompoundStateSpace / CompoundState hold a State** array of components with weights, so SE3 = RealVector(3) × SO3.

Memory is managed by the space, not by C++ constructors: allocState() / freeState() / copyState(). This lets OMPL allocate polymorphic states without knowing their type, and keeps planners generic.

The StateSpace interface (pure virtuals in bold) is the full set of operations a planner may perform on states:

Car-like spaces override distance/interpolate with analytic optimal curves: DubinsStateSpace (forward only, 6 word types LSL, RSR…) and ReedsSheppStateSpace (forward/backward, 48 candidate words). These are non-symmetric or non-holonomic, which matters for bidirectional planners (hasSymmetricInterpolate).

2.2 Constrained spaces (base/spaces/constraint)

When valid states lie on an implicit manifold \(F(q)=0\) (holding a tray level, closed chains), you supply a Constraint (\(F\) and its Jacobian). Three wrappers make that manifold look like an ordinary StateSpace:

• ProjectedStateSpace: sample/interpolate in ambient space, then Newton-project onto \(F=0\).
• AtlasStateSpace: lazily builds an atlas of tangent-space charts (AtlasChart); samples and interpolates in charts.
• TangentBundleStateSpace: a lazier atlas variant.

Any geometric planner then works unchanged. This is a good example of OMPL's design: planners never know.

2.3 SpaceInformation: the planner's view of the world

SpaceInformation bundles a StateSpace with a StateValidityChecker and a MotionValidator and forwards to them: isValid(s), checkMotion(s1,s2), distance, allocState, allocStateSampler, getMotionStates. Planners talk to si_ and nothing else.

• StateValidityChecker: isValid(state) → bool, optionally clearance(state). This is the only place OMPL touches geometry. You implement it with FCL/Bullet/MoveIt/VAMP or a lambda.
• MotionValidator: the default DiscreteMotionValidator checks the end state, then checks interior points along interpolate(a,b,t) in bisection order (a queue of index ranges, checking midpoints first). That order finds collisions in the middle early. It is also resolution-complete only: thin obstacles between checks can be missed, which is why the segment fraction matters. checkMotion(s1,s2,lastValid) also reports the last valid state (used for RRT "extend" and lazy planners).

2.4 ProblemDefinition, goals, objectives

• ProblemDefinition: start states, a Goal, an OptimizationObjective, and the solution list (planners push PlannerSolutions; getSolutionPath() returns the best one).
• Goals: Goal::isSatisfied(s, &distance). GoalRegion adds distanceGoal plus a threshold. GoalSampleableRegion can produce goal samples (needed for bidirectional planners and goal biasing). GoalStates is a finite set. GoalLazySamples uses a background thread that produces goal samples, e.g. from IK.
• OptimizationObjective: an abstract cost algebra. stateCost, motionCost, combineCosts (default +), isCostBetterThan (default <), identityCost, infiniteCost, costToGo heuristic, and allocInformedStateSampler. Implementations: PathLengthOptimizationObjective (default), MaximizeMinClearanceObjective, StateCostIntegralObjective, MechanicalWorkOptimizationObjective, MinimaxObjective, and MultiOptimizationObjective (weighted sum via operator+/operator*).

2.5 Planner, termination, status, data

class Planner {
  virtual PlannerStatus solve(const PlannerTerminationCondition &ptc) = 0;
  virtual void setup();          // read params, allocate NN structures
  virtual void clear();          // drop the tree/roadmap
  virtual void clearQuery();     // keep the roadmap, drop start/goal (multi-query planners)
  virtual void getPlannerData(PlannerData &) const;  // export graph for visualization/storage
  PlannerSpecs specs_;           // recognizedGoal, multithreaded, approximateSolutions, optimizingPaths, directed...
  ParamSet params_;              // string-keyed parameters ("range", "goal_bias") for benchmarking/MoveIt config
};

• PlannerTerminationCondition (ptc): a std::function<bool()> that is polled in the inner loop. timedPlannerTerminationCondition(seconds), exactSolnPlannerTerminationCondition, CostConvergenceTerminationCondition, and combinators (plannerOrTerminationCondition). The timed version evaluates on a separate thread that sets an atomic flag. Planners must poll ptc frequently. A port must keep this cooperative-cancellation contract.
• PlannerStatus: EXACT_SOLUTION, APPROXIMATE_SOLUTION, TIMEOUT, INVALID_START, INVALID_GOAL, UNRECOGNIZED_GOAL_TYPE, CRASH, ABORT, INFEASIBLE.
• PlannerData: a graph of vertices (states) and edges (weights/controls), backed by Boost.Graph (PlannerDataGraph.h). PlannerDataStorage serializes it with Boost.Serialization so roadmaps can be reused.

3. The planners: how they work, and where

3.1 Anatomy: RRT, line by line

geometric/planners/rrt/src/RRT.cpp solve() is the archetype. Paraphrasing:

add all valid start states to nearest-neighbor structure nn_
while !ptc():
    q_rand ← goal sample (prob goalBias_=0.05, if GoalSampleableRegion) else uniform sample
    q_near ← nn_.nearest(q_rand)
    if dist(q_near,q_rand) > range: q_new ← interpolate(q_near, q_rand, range/d) else q_new ← q_rand
    if si.checkMotion(q_near, q_new):
        add Motion{q_new, parent=q_near} to nn_
        if goal.isSatisfied(q_new, &d): solution ← it; break
        track best approximate solution by goal distance d
walk parent pointers back, build PathGeometric, pdef.addSolutionPath(path, approximate, d)

Every tree planner is a variation on these four primitives: sample, nearest, steer, check. range (maxDistance_) defaults to 0.2 × getMaximumExtent() via SelfConfig::configurePlannerRange (magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION).

3.2 Planner families

3.3 RRT* rewiring in practice

RRTstar.cpp: per iteration, the neighborhood is nn_->nearestK(motion, ceil(k_rrt_ * log(n+1))) if useKNearest_ (default), otherwise nearestR with \(r = \min(\text{range}, r_{rrt}(\log(n+1)/(n+1))^{1/d})\). calculateRewiringLowerBounds() computes \(k_{rrt}, r_{rrt}\) from the space dimension and getMeasure() and multiplies by rewire_factor (1.1). The planner chooses the parent minimizing cost-to-come, then rewires neighbors through the new node. Options include delayCC (sort before collision checking), tree pruning, informed sampling, and "new state rejection".

3.4 Kinodynamic planning (ompl/control)

When you can't steer between two states exactly (cars, drones, anything with dynamics), planners can only forward-propagate controls:

• ControlSpace / Control (mirrors StateSpace/State; RealVectorControlSpace), a ControlSampler, and a DirectedControlSampler (tries \(k\) controls and keeps the one that lands closest to a target).
• StatePropagator::propagate(state, control, duration, result): your dynamics. ODESolver adapts an ODE \(\dot q = f(q,u)\) using Boost.Odeint (ODEBasicSolver = RK4, ODEErrorSolver, ODEAdaptiveSolver = Cash-Karp 5(4)).
• control::SpaceInformation adds propagationStepSize and min/max control durations, and propagateWhileValid.
• PathControl is a sequence of (state, control, duration).
• Planners: control::RRT, KPIECE1, EST, PDST, SST, Syclop (a high-level discrete lead through a workspace decomposition, then a low-level RRT/EST), LTL (product automaton with spot for temporal-logic goals), and HyRRT/HySST (hybrid systems with jumps).

3.5 Multilevel (ompl/multilevel)

Plans on a sequence of increasingly detailed spaces \(X_1 \subset \dots \subset X_K\) (e.g. a sphere for the base, then full SE(3), then the arm). Solutions at lower levels bias sampling at higher levels via fiber bundles (QRRT, QRRT*, QMP). It's a research module.

4. Supporting machinery

4.1 Nearest neighbors: the real hot path

NearestNeighbors<T> is an abstract interface: add, remove, nearest, nearestK, nearestR, setDistanceFunction. Implementations:

• NearestNeighborsGNAT (default for single-threaded planners: GNATNoThreadSafety): Geometric Near-neighbor Access Tree, a metric tree with degree (8), min/max degree, and max points per leaf. It only needs a metric, so it works for SO(3), SE(3), and Dubins, unlike k-d trees. It supports removal (lazy, with periodic rebuild).
• NearestNeighborsSqrtApprox, NearestNeighborsLinear (brute-force reference; use it as the oracle when testing a port), and NearestNeighborsFLANN (optional).

In a typical RRT run on a cheap collision checker, NN queries are the largest time cost. In an expensive one (meshes), collision checks are. Profile before optimizing a port.

4.2 Path post-processing (geometric/PathSimplifier)

simplifyMax/simplify(ptc) run reduceVertices (random shortcutting: try to connect non-adjacent vertices directly), shortcutPath (shortcuts between random points on edges), collapseCloseVertices, perturbPath (cost-aware), and finally smoothBSpline. PathGeometric::interpolate(n) densifies the result for execution. PathHybridization merges multiple solutions (used by AnytimePathShortening).

4.3 Randomness

ompl::RNG wraps std::mt19937 with std::uniform_real_distribution and std::normal_distribution. Seeds: RNG::setSeed(s) sets the global seed generator (call it before any RNG is constructed), and setLocalSeed sets a per-instance seed. Special samplers: quaternion() (Shoemake's uniform method), uniformInBall, halfNormal, and uniformProlateHyperspheroid (for informed sampling).

4.4 Tools

• SimpleSetup: owns the SI and PDEF, picks a default planner via SelfConfig::getDefaultPlanner, and runs solve → simplifySolution. The default for geometric problems is RRTConnect, or LBKPIECE1 if the space has a default projection and the goal is sampleable with symmetric interpolation. Otherwise it uses RRT or KPIECE1. For control problems it uses KPIECE1 with a projection, else control::RRT.
• Benchmark: runs N planners × M runs with time and memory limits, records per-run properties (time REAL, solved BOOLEAN, path length, graph states…) plus progress properties (cost over time). It writes a .log file that scripts/ompl_benchmark_statistics.py turns into SQLite, viewed in Planner Arena. Use this format as the acceptance harness for a port.
• ParallelPlan, OptimizePlan, Profiler (OMPL_PROFILER macros), PlannerMonitor.

4.5 Integration points: who calls OMPL

• MoveIt / MoveIt 2 (moveit_planners_ompl): builds a ModelBasedStateSpace from URDF joint groups, implements StateValidityChecker with the planning scene (FCL), maps ompl_planning.yaml keys to Planner::params(), and handles path constraints with constrained spaces or approximation databases. The ParamSet string API is a de facto public contract because of this.
• VAMP (src/ompl/vamp): VampStateSpace, VampStateValidityChecker, VampMotionValidator. The motion validator checks a whole edge in SIMD lanes at once. Built by default (OMPL_BUILD_VAMP=ON) from the external/vamp submodule.
• OMPL.app: a separate repo with mesh-based rigid body planning (FCL/Assimp) and a GUI.

5. Build, bindings, tests

• CMake, C++17 (CMakeModules/OMPLCompilerSettings.cmake). Static by default, unless OMPL_BUILD_SHARED.
• Dependencies (CMakeModules/OMPLDependencies.cmake):
  • Required: Eigen3, Boost ≥ 1.68 (serialization, program_options, plus header-only graph, odeint, math, and dynamic_bitset).
  • Optional: Threads, Triangle (polygon decomposition for Syclop), FLANN ≥ 1.9.2, spot (LTL), yaml-cpp, Doxygen, Python (bindings).
  • Boost usage map (#include counts): math/constants 35, graph/* ~45 (adjacency_list, astar_search, dijkstra, incremental_components, graphviz), serialization/archive ~12, odeint (control), pending/disjoint_sets, dynamic_bitset, foreach, scoped_ptr (legacy).
• Python: py-bindings/ uses nanobind (submodule external/nanobind), hand-written per module (base, geometric, control, tools, util). Callbacks such as validity checkers can be Python callables, but that's slow because the GIL is crossed per state check.
• Tests: tests/ uses Boost.Test (~100 test cases) across base (state space sanity, samplers), geometric (2D grid-map problems resources/ppm/*.ppm, every planner must solve), control, datastructures (NN correctness versus linear), regression_tests (planner performance tracking), pytests, and vamp.

6. Recommended code-reading path

1. demos/RigidBodyPlanning.cpp, demos/StateSampling.cpp, demos/OptimalPlanning.cpp
2. base/State.h, base/StateSpace.h, base/spaces/RealVectorStateSpace.cpp, SO3StateSpace.cpp, SE3StateSpace.cpp (compound)
3. base/SpaceInformation.{h,cpp}, DiscreteMotionValidator.cpp, StateValidityChecker.h
4. base/ProblemDefinition.h, Goal*.h, Planner.{h,cpp}, PlannerTerminationCondition.cpp
5. geometric/planners/rrt/RRT.cpp, then RRTConnect.cpp, then RRTstar.cpp
6. geometric/planners/prm/PRM.cpp (Boost.Graph usage, multi-query)
7. datastructures/NearestNeighborsGNAT.h, NearestNeighborsLinear.h
8. geometric/PathSimplifier.cpp, geometric/SimpleSetup.cpp, tools/config/SelfConfig.cpp
9. geometric/planners/kpiece/KPIECE1.cpp + Discretization.h (projection/grid family)
10. control/SpaceInformation.cpp, ODESolver.h, control/planners/sst/SST.cpp
11. geometric/planners/informedtrees/BITstar.cpp + bitstar/* (largest, most intricate planner)
12. tools/benchmark/Benchmark.cpp, scripts/ompl_benchmark_statistics.py