✨ Selectable motion-planning backend (OMPL / cuRobo)

[correll]

Summary

Makes the arm joint-space motion planner swappable between OMPL (current, CPU) and cuRobo (CUDA GPU), selected by one config key. Adds a cuRobo backend, a robot config generated from the existing collision assets, and a GPU smoke test — without changing anything downstream of the planner.

Motivation: cuRobo gives GPU-accelerated, collision-aware planning (and a path to environment-aware reaching), but it requires a CUDA GPU and so cannot run in the headless CPU sim (Mac/Colima). This PR keeps OMPL as the default with automatic fallback, so the sim keeps working untouched while cuRobo becomes available on GPU hosts.

The seam

Everything downstream consumes one contract — PlannerClient.plan_configuration(...) → PlanResult (an N×14 reduced-joint waypoint array streamed by execute_path). A new factory picks the backend behind that contract, so frame_task_server, dual_arm_server, and execute_path are unchanged.

planner.backend:  ompl | curobo | auto
  ompl    -> ReducedJointPlanner (CPU, always available) — DEFAULT
  curobo  -> CuroboJointPlanner (CUDA GPU); fails loudly if no GPU
  auto    -> cuRobo when a CUDA GPU is present, else OMPL
             (also degrades to OMPL if cuRobo construction throws — planning never breaks)

Changes

Backend seam

• core/planner/planner_types.py (new) — ompl-free PlannerConfig/PlanResult (adds backend + curobo fields), re-exported from reduced_joint_planner for backward compat. Lets the cuRobo backend be imported/tested with neither ompl nor CUDA present.
• core/planner/backend.py (new) — create_joint_planner() + curobo_available(); the selection/fallback logic above.
• core/planner/curobo_planner.py (new) — CuroboJointPlanner, matching plan(start, goal, active_mask) → PlanResult exactly. Endpoint validation, active-mask projection (pin the untasked arm), and final revalidation mirror OMPL's guarantees (so execute_path's re-checks always pass). All cuRobo/CUDA imports are lazy; supports injecting an absolute urdf_path so the committed YAML stays host-agnostic.
• Wired the factory into the planner subprocess (planner_process.py); threaded backend/curobo config through _load_planner_config + debug.yaml.

cuRobo robot config — config/curobo/h1_2_handless_curobo.yml (new), generated from h1_2_handless_sphere.urdf + its SRDF so both backends share identical self-collision geometry:

Section	Contents	Derived from
collision_spheres	24 spheres / 16 links (pelvis, torso, both arms), exact centers + radii	fixed sphere joints in the sphere URDF
self_collision_ignore	39 parent-link pairs	SRDF disable_collisions (sphere-bearing links) + kinematic-ancestor closure
lock_joints	12 legs + torso, fixed at 0 (nominal stand)	BODY_JOINTS − ENABLED_JOINTS
cspace	the 14 arm joints, retract_config = home (zeros)	ENABLED_JOINTS, NAMED_CONFIGS['home']
base_link / ee_link	pelvis / left_wrist_yaw_link, both wrists + grasp frames tracked	URDF tree

• config/curobo/gen_curobo_cfg.py (new) — pure-stdlib, reproducible generator; re-run when the sphere URDF/SRDF change.
• config/curobo/smoke_test_curobo.py (new) — loads the YAML into cuRobo MotionGen and plans one motion; run first on a GPU host to shake out schema issues. Exits 0/1/2 for pass / plan-fail / no-cuRobo.

Tests — test/test_planner_backend.py (new): selection/fallback branches + the GPU-free cuRobo contract.

Key design decisions

• base_link = pelvis (not torso_link) so pelvis/torso spheres stay in the tree for arm-vs-body avoidance; this pulls torso_joint into the chain, hence it's in lock_joints.
• Torso locked at 0 — cuRobo's lock_joints are static at load, so if the torso is rotated at runtime the collision model drifts. Flagged inline; the clean fix later is syncing the locked value per-plan or adding torso to cspace.
• cspace = 14 movable joints, rest locked — the convention most likely to need a first-GPU-load tweak (some cuRobo versions want all 27 joints enumerated with lock_joints subtracting). Noted inline; regenerating for that layout is trivial.

Verified

• All new/changed Python files byte-compile.
• The host here has only numpy (no ompl/pinocchio/torch), so the full pytest suite must run in-container — but the selection/fallback logic and the cuRobo backend's pure-Python contract were exercised at runtime (stubbing the heavy package __init__s): 13/13 checks pass (every backend branch + cuRobo endpoint-validation, missing-robot_cfg, active-mask projection, column reorder).
• h1_2_handless_curobo.yml parses as valid YAML; shapes cross-checked (14/14 cspace, collision_spheres keys == collision_link_names, 13 locked joints).

NOT verified (needs a GPU)

No CUDA in this environment. The sphere geometry, ignore pairs, and joint mapping are exact, but cuRobo schema acceptance and the MotionGen planning path are unvalidated. curobo_planner.py and smoke_test_curobo.py follow cuRobo's documented MotionGen API and use getattr fallbacks where accessor names vary by version.

Enabling cuRobo on a GPU host

1. Install cuRobo (source build against matched torch + CUDA).
2. python3 config/curobo/smoke_test_curobo.py --urdf /abs/path/h1_2_handless_sphere.urdf → expect [smoke] PASS and active DOF = 14. Fix any schema mismatch here.
3. Set in the controller config:

   planner:
     backend: auto        # or curobo
     curobo:
       robot_cfg: config/curobo/h1_2_handless_curobo.yml
       urdf_path: /abs/path/CL_Assets/ros_assets/h1_2_handless_sphere.urdf

4. auto keeps OMPL as the fallback, so the same config is safe on the CPU sim.

Test plan

• In-container: pytest test/test_planner_backend.py (+ existing test_reduced_joint_planner.py unaffected).
• CPU sim regression: backend: auto on Mac/Colima still uses OMPL; rob_pose planning unchanged.
• GPU host: smoke test passes; then backend: curobo plans a named-config move end-to-end.

🤖 Generated with Claude Code