Metadata-Version: 2.1
Name: nav-planning
Version: 0.1.0
Summary: Motion planning for the LoCoBot and the Ridgeback with a pan-tilt camera (OMPL + VAMP)
Project-URL: Homepage, https://github.com/H-tr/Nav-Planning
Project-URL: Repository, https://github.com/H-tr/Nav-Planning
Project-URL: Issues, https://github.com/H-tr/Nav-Planning/issues
Requires-Python: >=3.10
Requires-Dist: numpy
Requires-Dist: pin
Provides-Extra: dev
Requires-Dist: pre-commit; extra == "dev"
Requires-Dist: pybullet; extra == "dev"
Requires-Dist: pytest>=8.0; extra == "dev"
Description-Content-Type: text/markdown

# 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](https://github.com/H-tr/Fetch-Planning) and
[Autolife-Planning](https://github.com/H-tr/Autolife-Planning): same
public API, same module layout, same build and release pipeline.

```bash
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](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)

| Index | LoCoBot            | Ridgeback          | Notes                                  |
|------:|--------------------|--------------------|----------------------------------------|
|   0   | `base_x_joint`     | `base_x_joint`     | world frame                            |
|   1   | `base_y_joint`     | `base_y_joint`     | world frame                            |
|   2   | `base_theta_joint` | `base_theta_joint` | heading (periodic)                     |
|   3   | `pan`              | `ptu_pan`          | ±90° (LoCoBot), ±168° (PTU-5)          |
|   4   | `tilt`             | `ptu_tilt`         | −90°…+75° (LoCoBot), −90°…+30° (PTU-5) |

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](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

```python
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:

```python
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`.

| Subgroup | Leg | FLASK median / p95 | Duration | RRT-Connect median / p95 |
|---|---|---|---|---|
| locobot_base | room 2 | 0.4 / 0.4 ms | 22.5 s | 2.1 / 3.5 ms |
| locobot_base | behind pillar | 51.7 / 106.6 ms | 17.3 s | 2.5 / 10.3 ms |
| locobot_base | table | 12.9 / 67.9 ms | 8.2 s | 0.8 / 1.3 ms |
| locobot_whole_body | room 2 | 0.4 / 0.4 ms | 22.5 s | 2.4 / 9.2 ms |
| locobot_whole_body | behind pillar | 52.5 / 94.1 ms | 16.7 s | 3.3 / 8.0 ms |
| locobot_whole_body | table | 11.1 / 58.0 ms | 6.6 s | 0.9 / 1.2 ms |
| ridgeback_base | room 2 | 0.6 / 0.7 ms | 8.2 s | 5.1 / 29.7 ms |
| ridgeback_base | behind pillar | 38.4 / 152.7 ms | 9.1 s | 8.4 / 35.5 ms |
| ridgeback_base | table | 0.2 / 0.2 ms | 3.2 s | 0.5 / 1.7 ms |
| ridgeback_whole_body | room 2 | 0.7 / 0.7 ms | 8.6 s | 7.2 / 51.9 ms |
| ridgeback_whole_body | behind pillar | 32.7 / 130.7 ms | 9.7 s | 12.9 / 48.1 ms |
| ridgeback_whole_body | table | 0.2 / 0.2 ms | 3.4 s | 1.0 / 1.6 ms |

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](docs/planning/kinodynamic.md#results-on-the-demo-scene).

---

## 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

```bash
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](https://github.com/hungpham2511/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.
