Nav-Planning

Motion planning for two mobile robots with a pan-tilt camera — the Trossen LoCoBot (iRobot Create 3 differential drive) and the Clearpath Ridgeback (holonomic) — with VAMP SIMD collision checking. A sister library of Fetch-Planning and Autolife-Planning: same public API, same module layout, same build and release pipeline.

pip install nav-planning

Motivation

Fetch-Planning and Autolife-Planning wrap OMPL over a VAMP collision backend for one robot each. Nav-Planning ports the same stack to two navigation robots that look around with a camera:

1. Two robots, one API — create_planner("locobot_base") or create_planner("ridgeback_whole_body"); the low-level extension takes the robot at construction, OmplVampPlanner("locobot") / OmplVampPlanner("ridgeback"), and otherwise offers the calls of Autolife-Planning's OmplVampPlanner.
2. Nonholonomic and holonomic bases — the LoCoBot's base (x, y, θ) is planned in a Reeds-Shepp space (forward, reverse, no sliding), the Ridgeback's in a plain SE(2) space (x, y and θ independent). Motion checks follow the space's own curves.
3. Kinodynamic planning — MotionPlanner.plan_kinodynamic (FLASK, flatness-based kinodynamic RRT-Connect) returns time-parameterised trajectories within the joint and base velocity / acceleration limits: exactly nonholonomic for the LoCoBot, within the per-axis body-frame limits of the Ridgeback's controller. See docs/planning/kinodynamic.md.
4. Collision models from the official descriptions — both robots go through the template pipeline: foam spheres + SRDF → cricket FK/CC code → VAMP robot models.

Architecture

┌───────────────────────────────────────────────────────────────┐
│                     nav_planning/ (Python)                    │
│                                                               │
│   create_planner("locobot_base" | "ridgeback_whole_body" ...) │
│        │                                                      │
│        ▼                                                      │
│   MotionPlanner  ── plan() / plan_kinodynamic()               │
└────────┼──────────────────────────────────────────────────────┘
         │
┌────────▼──────────────────────────────────────────────────────┐
│              _ompl_vamp  (C++/nanobind extension)             │
│                                                               │
│   OmplVampPlanner("locobot")     OmplVampPlanner("ridgeback") │
│   Reeds-Shepp SE(2) (+ R^2)      SE(2) (+ R^2)                │
│   FLASK: unicycle flat outputs   FLASK: x, y, θ flat outputs  │
│                                                               │
│   OMPL planners (RRT-Connect, BIT*, KPIECE, PRM, …);          │
│   validity + motion checks along the base curves              │
└────────┼──────────────────────────────────────────────────────┘
         │
┌────────▼──────────────────────────────────────────────────────┐
│              VAMP backend  (header-only, SIMD)                │
│   third_party/vamp → H-tr/vamp (unmodified)                   │
│   vamp::robots::{Locobot,Ridgeback}{WholeBody,Base}           │
│   (ext/ompl_vamp/include/vamp/robots/, generated by cricket)  │
└───────────────────────────────────────────────────────────────┘

Robot layout (5 DOF each)

Planning subgroups (PLANNING_SUBGROUPS in nav_planning/locobot.py and nav_planning/ridgeback.py):

• locobot_base, ridgeback_base — 3 DOF, the base; pan and tilt held
• locobot_whole_body, ridgeback_whole_body — 5 DOF, base + camera (locobot and ridgeback are aliases)

The robots (docs/robots.md):

• LoCoBot — the arm-less locobot_base on the iRobot Create 3 with the pan-tilt RealSense D435 tower, from the official interbotix_xslocobot_descriptions (ROS 2 Humble branch), without the optional lidar. 312 collision spheres.
• Ridgeback — the official ridgeback_description. The Ridgeback has no head; Nav-Planning adds a camera mast: a 40 × 40 mm extrusion, 0.70 m tall on the payload deck, carrying a FLIR PTU-5 (official flir_ptu_description, base 1.006 m above the floor) and an Intel RealSense D435 (official realsense2_description). 309 collision spheres.

Usage

import numpy as np
from nav_planning.planning import create_planner
from nav_planning.types import KinodynamicConfig, PlannerConfig

planner = create_planner(
    "locobot_whole_body",                # or "ridgeback_whole_body", "*_base"
    config=PlannerConfig(planner_name="rrtc", time_limit=1.0),
    pointcloud=cloud,                    # (N, 3) float32, world frame
)
planner.set_joint_limits({"base_x_joint": (-1.0, 7.0), "base_y_joint": (-3.0, 3.0)})

start = np.array([0.0, 0.0, 0.0, 0.0, 0.0])   # x, y, theta, pan, tilt
goal = np.array([6.0, 0.0, np.pi, 0.8, 0.4])

# Geometric path (densified along the base's curves).
result = planner.plan(start, goal)

# Time-parameterised, dynamically feasible trajectory.
kino = planner.plan_kinodynamic(start, goal, config=KinodynamicConfig(time_limit=1.0))
times, q, qd, qdd = kino.trajectory.sample_uniform(dt=0.01)
twist = kino.trajectory.sample_base_twist(times)   # (T, 3): v_x, v_y, omega

The low-level extension, driven the way PHANTOM drives Autolife-Planning's:

from nav_planning._ompl_vamp import OmplVampPlanner

planner = OmplVampPlanner("locobot")         # or "ridgeback"; full body, 5 DOF
lower, upper = planner.default_joint_limits()
planner.set_joint_limits(lower, upper)
planner.set_subgroup([0, 1, 2], full_configuration)
planner.add_pointcloud(points_float32_nx3, 0.0)
result = planner.plan(start, goal, planner_name="rrtc", time_limit=1.0,
                      simplify=True, interpolate=False)
result.solved, result.path                   # path: (N, dof) numpy array

# FLASK, with the limits of nav_planning/locobot.py (see
# MotionPlanner._kinodynamic_settings for the full set of fields):
settings = KinodynamicSettings()             # from nav_planning._ompl_vamp
settings.max_velocity, settings.max_acceleration = [], []   # base subgroup
settings.base_max_speed, settings.base_max_acceleration = 0.46, 0.9
settings.base_max_yaw_rate, settings.base_max_yaw_acceleration = 1.9, 7.725
kino = planner.plan_kinodynamic(start, [], goal, 1.0, settings)
kino.solved, kino.trajectory                 # trajectory: FlatTrajectory

With the base active, consecutive waypoints of an uninterpolated path are joined by the base's own curve — a Reeds-Shepp curve for the LoCoBot, a straight line with shortest-arc heading for the Ridgeback — not by a straight line in joint space; interpolate=True (or interpolate_path) samples those curves.

Results on the demo scene

examples/planning/kinodynamic.py: two rooms joined by a 1.2 m doorway, a table, a low shelf and two pillars (96k points, point radius 0); 20 trials per leg. RRT-Connect is the geometric planner as PHANTOM calls it (simplify=True, interpolate=False). Measured with pixi run python examples/planning/kinodynamic.py at commit 38b824f on 2026-10-19, single-threaded on an AMD EPYC 7543; raw output in /nas/pollux/tianrun/nav_geo/demo_scene_38b824f.txt.

Every plan succeeded (20/20). Sampled every 2 ms, the largest FLASK velocity or acceleration was 1.000 × its limit, and the LoCoBot's lateral speed was exactly zero. Collisions between samples are in docs/planning/kinodynamic.md.

Project layout

Nav-Planning/
├── CMakeLists.txt              # two extensions: _ompl_vamp, _time_parameterization
├── pyproject.toml              # scikit-build-core
├── pixi.toml                   # conda env + build tasks
├── .gitmodules                 # vamp, ompl, cricket, foam
│
├── nav_planning/               # Python package (mirrors fetch_planning/)
│   ├── locobot.py              # LoCoBot layout, limits, subgroups, …
│   ├── ridgeback.py            # Ridgeback layout, limits, subgroups, …
│   ├── types/                  # SE3Pose, PlannerConfig, KinodynamicConfig, …
│   ├── trajectory/             # TOPP-RA + kinodynamic trajectories
│   ├── kinematics/             # Pinocchio forward kinematics (camera pose)
│   ├── planning/               # MotionPlanner, create_planner
│   ├── envs/                   # PyBullet scene wrapper
│   └── resources/robot/        # locobot/, ridgeback/: URDFs, SRDFs, meshes
│
├── ext/
│   ├── ompl_vamp/              # nanobind OMPL↔VAMP bridge + FLASK
│   │   └── include/vamp/robots/ # generated VAMP robot models
│   └── time_parameterization/  # vendored TOPP-RA C++ core (nanobind)
│
├── scripts/                    # description → spheres → FK pipeline
├── examples/planning/          # demo scene; geometric, kinodynamic, TOPP-RA
├── tests/                      # pytest suite
└── third_party/                # vamp, ompl, cricket, foam

Dependencies

• Build: cmake ≥ 3.20, ninja, C++17 compiler, nanobind, Boost (serialization + filesystem), Eigen3
• Runtime: python ≥ 3.10, numpy, pinocchio (pin)
• Optional: pybullet (visualization)

Pixi handles all of this — pixi install builds the package into the environment.

Testing

pixi run test

.github/workflows/test.yml runs the same suite on every push and pull request against main; pip-compat.yml builds the manylinux wheels and runs the suite against each of them in a fresh venv.

License & attribution

• VAMP is from the Kavraki Lab (BSD); third_party/vamp is unmodified upstream and the robot models live in ext/ompl_vamp/include/vamp/robots/.
• The time parameterizer vendors the C++ core of toppra (MIT, © Hung Pham) unmodified under ext/time_parameterization/toppra/.
• The robot meshes come from interbotix_xslocobot_descriptions (BSD), irobot_create_description (BSD), ridgeback_description (BSD), flir_ptu_description (BSD-3-Clause) and realsense2_description (Apache 2.0).
• Everything else © 2026 H-tr, same license as Fetch-Planning.