The problem
Sampling-based planners on a 7-DOF arm in cluttered scenes run into the usual joint-limit-and-narrow-passage trap. Graph-of-Convex-Sets (GCS) planners promise globally optimal paths through unions of convex regions, but the wins are theoretical until you (a) actually grow the regions, (b) solve the SOCP, (c) refit the result into a smooth trajectory MoveIt2 can execute, and (d) make repeat queries cheap enough to use online.
Approach
A complete MoveIt2 plugin written in C++:
- IRIS-ZO to grow convex regions in C-space; farthest-point seeding picks where to grow next so coverage doesn't waste polytopes on already-covered volume.
- SOCP relaxation of the discrete shortest-path-through-regions problem, solved via CVXPY with the relaxation gap measured per query.
- C¹ Bézier trajectory optimization to fit a smooth trajectory through the chosen region sequence — continuous joint velocities at every region boundary.
- Caching: regions, polytope intersections, and pre-solved nominal trajectories are persisted so repeat queries on the same workspace skip the slow growth phase entirely.
I benchmarked the seeding strategy against a PointNet regressor (predicts good seeds from voxelized scene) and a conditional VAE (samples seeds conditioned on goal pose) on the same 20 scenes.
Result
- 28 s → 2.4 s on repeat queries — 12× speedup from the cache without changing any path quality
- −9.8% path cost with farthest-point seeding vs random seeding, averaged over 20 cluttered scenes
- Beats both the PointNet regressor (−4.1%) and the conditional VAE (−6.2%) on the same metric, with no learning step required
Tech notes
- Plugin host: MoveIt2 / OMPL plugin interface, C++17
- Region growth: IRIS-ZO with farthest-point seeding; coverage check via polytope intersection volume
- Solver: SOCP via CVXPY (Python sidecar), relaxation gap logged per query
- Smoothing: C¹ Bézier per region, knot continuity at boundaries
- Hardware: Franka Emika Panda, libfranka, 1 kHz commanded