Planning Shorter Paths in Graphs of Convex Sets by Undistorting Parametrized Configuration Spaces

Shruti Garg, Thomas Cohn, Russ Tedrake

Optimization based motion planning provides a useful modeling framework through various costs and constraints. Using Graph of Convex Sets (GCS) for trajectory optimization gives guarantees of feasibility and optimality by representing configuration space as the finite union of convex sets. Nonlinear parametrizations can be used to extend this technique to handle cases such as kinematic loops, but this distorts distances, such that solving with convex objectives will yield paths that are suboptimal in the original space. We present a method to extend GCS to nonconvex objectives, allowing us to "undistort" the optimization landscape while maintaining feasibility guarantees. We demonstrate our method's efficacy on three different robotic planning domains: a bimanual robot moving an object with both arms, the set of 3D rotations using Euler angles, and a rational parametrization of kinematics that enables certifying regions as collision free. Across the board, our method significantly improves path length and trajectory duration with only a minimal increase in runtime.

Read the Preprint

Results Video:

Meshcat Recordings of Constrained Bimanual Motion Plans

Click each image below to open an interactive meshcat recording of the trajectory. The top row shows the results using our method (end effector path marked in blue), and the bottom row shows the original result before applying our postprocessing strategy (end effector path marked in red).