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

TL;DR

The paper addresses the mismatch between nonconvex objective landscapes and convex feasibility in Graphs of Convex Sets (GCS) for motion planning, caused by nonlinear parametrizations of the configuration space. It proposes a framework that isolates the nonconvex objective through nonlinear coordinate changes α and optimizes it on the parametrized space Q using Projected Gradient Descent (PGD) while maintaining feasibility via convex projections. The approach is demonstrated across constrained bimanual IK, planning over SO(3) with Euler angles, and rational kinematics with certification, yielding substantial reductions in path length and trajectory time with modest runtime increases. This work broadens GCS applicability to richer, nonconvex objectives and certified planning, offering practical improvements for complex robotic systems while preserving the robustness guarantees of convex optimization.

Abstract

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. Website: https://shrutigarg914.github.io/pgd-gcs-results/

Figures (7)

• Figure 1: Experiments include constrained bimanual motion planning between shelves (top) and certifiable 7DoF KUKA iiwa tr aj ectories between bins ( bottom ). The red path is the original result, and the blue path is our improved result.
• Figure 2: A stereographic projection about $N$ projects the bottom of the black rectangle as being smaller than the top. An optimal distance planner operating in the post-projection (parametrized) space would favor the bottom despite the sides being equal in actuality. Image generated using animation-stereo.
• Figure 3: Optimizing jointly for curvature and distance yields quicker trajectories but longer distances--the curvature-regularized path is farther from the shelf.
• Figure 4: Paths become more centered as the nonconvex objective accounts for the distance traveled by both arms. The original convex objective just accounts for the controlled arm.
• Figure 5: Comparing the distributions of relative error of paths with respect to the SLERP distance between start and goal orientations. The PGD significantly improves the results of the Euler angles parametrization.