Motion Planning¶

OMPL frontend

All of OMPL's mature sampling-based planners (30 exposed), driven by one uniform Python call. No glue code to swap planners — change a string.
VAMP backend

Collision checking runs through VAMP's SIMD-vectorised sphere pipeline. Point-cloud obstacles are broadphase-indexed.
C++ hot path

Python calls plan(start, goal) once. Everything inside — sampling, NN queries, collision checks, path simplification — runs in C++ behind a single nanobind call.

Architecture¶

         Python
           │  planner.plan(start, goal)
           ▼
    ┌─────────────────────────────────────────┐
    │     OMPL SimpleSetup  (C++)              │
    │  ┌────────────┐  ┌──────────────────┐    │
    │  │  Planner   │→ │ MotionValidator  │    │
    │  │  RRT-C,    │  │  checkMotion     │    │
    │  │  BIT*, …   │  └────────┬─────────┘    │
    │  └────────────┘           │              │
    │                           ▼              │
    │                  ┌─────────────────┐     │
    │                  │ VAMP SIMD       │     │
    │                  │  sphere-cloud   │     │
    │                  │  collision      │     │
    │                  └─────────────────┘     │
    └─────────────────────────────────────────┘

The frontend is vanilla OMPL — ProjectedStateSpace, StateCostIntegralObjective, SimpleSetup. The collision checker is the only replaced component; it swaps OMPL's per-state user callback for a VAMP pipeline that batches many sphere/point checks into a single AVX2 kernel.

Supported planners¶

The planner_name field of PlannerConfig accepts any of:

Feasibility (single-query, returns on first solution)Asymptotically optimal (keep refining until budget)

Family	Planners
RRT	`rrtc`, `rrt`, `trrt`, `bitrrt`
KPIECE	`kpiece`, `bkpiece`, `lbkpiece`
PRM	`prm`, `lazyprm`
Exploration	`est`, `biest`, `sbl`, `stride`, `pdst`

Family	Planners
RRT*	`rrtstar`, `informed_rrtstar`, `rrtsharp`, `rrtxstatic`
Informed trees	`bitstar`, `abitstar`, `aitstar`, `eitstar`, `blitstar`
FMT	`fmt`, `bfmt`
PRM*	`prmstar`, `lazyprmstar`
Near-optimal	`lbtrrt`, `spars`, `spars2`

Benchmarks¶

Median / p95 planning time of a single search (simplification and interpolation disabled), N = 20 runs per cell, from the home pose to a sampled collision-free goal around the table point cloud of examples/planning/motion.py. Only exact solutions count; every feasibility run found one.

Feasibility planners — time to first solution¶

Planner	Arm (7 DOF)	Base (3 DOF)	Whole body (10 DOF)
`rrtc`	2.78 / 4.28 ms	1.16 / 1.30 ms	1.17 / 2.28 ms
`rrt`	6.00 / 423.95 ms	3.30 / 6.62 ms	11.71 / 71.21 ms
`kpiece`	22.29 / 165.93 ms	3.32 / 8.52 ms	30.46 / 77.76 ms
`bkpiece`	29.87 / 40.40 ms	6.48 / 9.66 ms	103.25 / 173.67 ms
`lbkpiece`	48.01 / 99.57 ms	4.42 / 6.59 ms	235.99 / 341.09 ms
`prm`	20.65 / 59.59 ms	66.19 / 224.49 ms	14.94 / 329.68 ms
`lazyprm`	7.09 / 14.90 ms	1.16 / 1.28 ms	5.96 / 17.00 ms
`est`	6.57 / 21.22 ms	2.73 / 8.53 ms	6.96 / 73.56 ms
`biest`	4.91 / 7.73 ms	1.70 / 4.43 ms	4.90 / 10.70 ms
`sbl`	9.13 / 13.90 ms	2.21 / 3.34 ms	17.07 / 36.31 ms
`stride`	41.96 / 175.51 ms	3.28 / 14.87 ms	26.61 / 73.45 ms

The KPIECE family, STRIDE and PDST use the motion check that reports the last valid state of a failed motion, which checks a failed motion one sample at a time.

Asymptotically-optimal planners — median path cost after a 1.0 s budget¶

Planner	Arm	Base	Whole body
`rrtstar`	9.94 (19/20 solved)	7.04	18.05
`bitstar`	9.63 (19/20 solved)	8.48	17.30
`aitstar`	11.45	9.63	19.87
`eitstar`	9.77	6.72	16.49
`fmt`	9.63	8.68	15.48

Measurement

pixi run -e dev python scripts/benchmarks/motion_planning.py, commit 1f2ac01, 2026-10-19, AMD EPYC 7543 (128 logical CPUs, shared with other jobs, load 8–11) · Linux 6.8 · Python 3.14.7. p95 is the script's nearest-rank value. Raw output: /nas/pollux/tianrun/wheelchair_flask/geo_benchmark.txt.

Minimal example¶

import numpy as np

from wheelchair_planning.wheelchair import HOME_JOINTS
from wheelchair_planning.planning import create_planner
from wheelchair_planning.types import PlannerConfig

planner = create_planner(
    "wheelchair_arm",
    config=PlannerConfig(planner_name="rrtc", time_limit=1.0),
    base_config=HOME_JOINTS.copy(),
    pointcloud=obstacle_cloud,           # (N, 3) np.float32
)

start = planner.extract_config(HOME_JOINTS)
goal = planner.sample_valid()
result = planner.plan(start, goal)
print(result.success, result.planning_time_ns * 1e-6, "ms")

Configuring a plan¶

Every plan is driven by a single PlannerConfig dataclass. The defaults are tuned for interactive use — a millisecond-scale single-query plan with a dense, simplified path — so you rarely need to touch anything except planner_name and time_limit. The knobs that matter in practice:

PlannerConfig(
    # Which OMPL planner to run — see "Supported planners" above.
    planner_name="rrtc",

    # Wall-clock ceiling for the search.  Feasibility planners return
    # on the first solution, so this is just a safety cap.  AO planners
    # (rrtstar, bitstar, …) run until the budget to refine cost.
    time_limit=1.0,

    # Inflation applied to every obstacle point in the VAMP broadphase.
    # Larger = more conservative, but slows the search.  0.01 m is a
    # good default for ~1 cm-resolution scans.
    point_radius=0.01,

    # After the raw path, run OMPL's SimpleSetup shortcutter.  Removes
    # jagged detours the sampler produced without changing homotopy.
    # Turn this OFF for constrained / cost planners — the default
    # shortcutter takes straight-line shortcuts that ignore custom
    # constraints and costs.
    simplify=True,

    # Resample the (simplified) path densely: each edge of length d
    # gets ceil(d * resolution) segments; resolution=0.0 falls back to
    # OMPL's default longest-valid-segment fraction, usually too sparse
    # for control.  The default (64.0) gives ~64 waypoints per unit of
    # state-space distance, which is smooth enough for a 100 Hz control
    # loop and robust to low-frequency replanning.
    interpolate=True,
    resolution=64.0,

    # Joint-range overrides by joint name, (lower, upper) in metres for
    # the virtual base joints and radians otherwise.  Joints left out
    # keep the range compiled into the robot model (the base spans
    # ±10 m); the planner samples only inside these ranges.
    joint_limits={"Joint_Virtual_X": (-2.0, 3.0), "Joint_Virtual_Y": (-1.5, 1.5)},
)

planner.set_joint_limits(limits) changes the ranges of an existing planner with the same semantics: joints left out take the compiled range (None restores every one), an unknown name or a range that is not finite with lower < upper raises ValueError, and the ranges are kept across set_subgroup. planner.bounds gives the active joints' ranges as (lower, upper) arrays and planner.joint_limits every joint's range by name.

Typical recipes:

Interactive — fast feasibilityHigh-quality, asymptotically optimalConstrained / cost planner

PlannerConfig(
    planner_name="rrtc",          # returns on first solution
    time_limit=0.5,
    simplify=True,
    resolution=64.0,
)

PlannerConfig(
    planner_name="bitstar",       # keeps refining until budget
    time_limit=2.0,
    simplify=True,
    resolution=128.0,             # denser output for smoother control
)

PlannerConfig(
    planner_name="rrtstar",
    time_limit=5.0,
    simplify=False,               # the shortcutter ignores custom cost/constraint
    resolution=64.0,
)

Post-hoc simplify / interpolate¶

simplify and interpolate run inside plan(...) by default, but the same pipeline is exposed as standalone methods on MotionPlanner — handy when you want to:

Plan once with raw output (simplify=False, interpolate=False) and apply them later.
Re-densify an old path at a different resolution without replanning.
Keep the search raw, smooth only at display time — the controller consumes the original waypoints, the visualiser gets a densely interpolated copy.
Drop simplification for constrained / cost plans (which you want) but still apply it manually to specific segments where the shortcutting is known-safe.

planner = create_planner(
    "wheelchair_arm",
    config=PlannerConfig(simplify=False, interpolate=False),
)
result = planner.plan(start, goal)                     # unsimplified, raw
smooth = planner.simplify_path(result.path, time_limit=1.0)
dense  = planner.interpolate_path(smooth, resolution=128.0)     # or count=200, resolution=0.0

Both methods reuse the planner's collision environment and constraint set. simplify_path only consults the motion validator (geometric shortcuts — not cost-aware, same caveat as the in-plan flag). interpolate_path runs StateSpace::interpolate on the existing edges, so it stays on the constraint manifold for projected state spaces and does not perform collision checks itself.

Standalone collision checking¶

The same VAMP SIMD pipeline that the planner drives per motion edge is exposed directly — useful for filtering sampled goals, validating a trajectory produced outside the planner, or scoring a whole candidate batch of configurations at once.

planner.validate(cfg)                   # single config → bool
planner.validate_batch(cfgs)            # (N, ndof) → (N,) bool array

validate_batch packs up to rake distinct configurations directly into a VAMP ConfigurationBlock<rake> and runs one fkcc<rake> call per block — the same primitive the motion validator uses for interpolated samples along an edge, fed independent configs per lane instead. Per-config result is preserved: when a packed block fails, only that block falls back to per-lane checks.

A block of valid configurations costs one SIMD sweep; a block with an invalid lane pays the per-lane fallback.

# Example: filter 1000 candidate goals by validity.
goals = np.random.uniform(lo, hi, size=(1000, planner.num_dof))
mask = planner.validate_batch(goals)
good_goals = goals[mask]

More¶

Subgroup planning

Plan over a slice of the 10-DOF body — the arm, the base, or the whole body — with the remaining joints pinned to any 10-DOF stance.
Manifold planning

Hard task-space equality constraints written as CasADi expressions (planes, rails, orientation locks, couplings). Compiled once, cached, and injected into OMPL's ProjectedStateSpace.
Cost-space planning

Soft path-integral costs — the same CasADi authoring model, but now the constraint becomes a preference. Drives OMPL's asymptotically-optimal planners.
Time parameterization

Convert geometric paths into time-optimal trajectories with per-joint velocity and acceleration limits, using TOPP-RA.
Kinodynamic planning

FLASK: plan a time-parameterised trajectory directly, with joint and base velocity / acceleration limits and an exactly nonholonomic (diff-drive) base. Track it with feed-forward + PID.