Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets

Authors: Thomas Cohn, Mark Petersen, Max Simchowitz, Russ Tedrake · RSS 2023

Licences

Open Access
Paper: CC BY 4.0

Everything below draws on the full paper under its CC BY 4.0 licence; its figure is credited to the authors.

TL;DR

Treats joint spaces with wrap-around (mobile bases, continuous joints) as flat manifolds so the GCS shortest-path planner still gives globally optimal, collision-free paths.

Why it matters

Mobile manipulators and arms with unlimited base joints can now use a planner that certifies every point of the trajectory is collision-free and, when solved exactly, optimal within the certified free space. The paper also proves that positive curvature (for example SO(3) ball joints or free-floating bodies) breaks convexity, which marks where this approach cannot go.

Problem

The GCS planner of Marcucci et al. finds collision-free, globally optimal paths through a decomposition of free configuration space into convex sets, but only for Euclidean configuration spaces. A mobile base that can turn a full 360°, or a continuous revolute joint, wraps around, so its configuration space is not Euclidean. Adding artificial joint limits or treating angles as unbounded reals either makes the optimum infeasible or needs prior knowledge of the answer and many duplicated sets.

Proposed solution

The authors model the configuration space as a Riemannian manifold and define a Graph of Geodesically-Convex Sets (GGCS), proving it keeps the guarantees of ordinary GCS. For flat (zero-curvature) manifolds, which cover any robot built from 1-DoF joints plus an SE(2) base, each set is mapped through a local-isometry chart to Euclidean space and the problem reduces to a standard mixed-integer convex GCS problem with affine transition constraints. Regions are grown with IRIS and bounded per joint by the convexity radius, and piecewise Bézier trajectories with continuity constraints carry over.

Key insights

SE(2), every 1-D manifold and products of flat manifolds are flat, so robots made of single-DoF joints and a mobile base fit the method; 3-DoF ball joints do not
Bounding each IRIS region per joint by the convexity radius guarantees the region stays geodesically convex when mapped back to the manifold
On a flat space with globally aligned coordinates the transition maps are affine, so endpoint-matching and derivative-continuity constraints stay convex
Even one point of positive curvature makes the Riemannian distance function nonconvex in every neighbourhood of that point (Theorem 5)

Main results

Point robot on a torus: optimal wrap-around path in 0.79 s; 5-joint planar arm with continuous joints: plans in 5.36 s and 4.63 s, one of which turns the middle joint more than 360°
Planar arm: relax-and-round paths of length 7.749 and 8.448 vs branch-and-bound optima of 7.274 and 8.008
KUKA iiwa with a continuous base joint (simulation): 14 mug-sorting motions using 18 convex regions, averaging 25.75 s per motion (range 4.63–50.30 s)
PR2 (simulation, 15-D configuration space), 7 shelf-reaching tasks: GGCS paths were collision-free in every case, e.g. 1M→4M length 3.875 vs PRM 14.554; Drake-Trajopt and PRM+Trajopt sometimes gave shorter paths but those collided slightly
PR2 online planning took 25.51–66.15 s for GGCS-Planner, vs 0.49–0.54 s for the PRM, 2.26–15.23 s for Drake-Trajopt and 11.61–40.10 s for PRM+Trajopt
PR2 offline set-up: an IRIS region took 30.20 s on average (8.56–75.42 s), all regions 156.63 s in parallel; building the 100,000-sample PRM took 124.39 s

Limitations

The authors state the method cannot handle nonconvex costs and constraints such as nonlinear dynamics and closed kinematic chains, nor ball joints
Positive-curvature spaces such as SO(3) are excluded, because the distance function is provably nonconvex there
Optimality holds only within the 'certified' free space covered by the regions; performance depends strongly on the choice of regions, and for the PR2 some intermediate regions were added by hand
Online planning times for the PR2 (25.51–66.15 s) were higher than every baseline compared
All robot results are in simulation; the authors list bimanual planning with implicit kinematic constraints and planning through contact as future work

Field impact

Finalist for the RSS 2023 Best Paper Award, with 44 citations. The authors publish interactive recordings of every trajectory on a results website.

Abstract

Computing optimal, collision-free trajectories for high-dimensional systems is a challenging problem. Sampling-based planners struggle with the dimensionality, whereas trajectory optimizers may get stuck in local minima due to inherent nonconvexities in the optimization landscape. The use of mixed-integer programming to encapsulate these nonconvexities and find globally optimal trajectories has recently shown great promise, thanks in part to tight convex relaxations and efficient approximation strategies that greatly reduce runtimes. These approaches were previously limited to Euclidean configuration spaces, precluding their use with mobile bases or continuous revolute joints. In this paper, we handle such scenarios by modeling configuration spaces as Riemannian manifolds, and we describe a reduction procedure for the zero-curvature case to a mixed-integer convex optimization problem. We demonstrate our results on various robot platforms, including producing efficient collision-free trajectories for a PR2 bimanual mobile manipulator.

Open Source Robotics