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

TL;DR

This paper introduces a method to extend Graph of Convex Sets (GCS) for nonconvex objectives in motion planning, effectively 'undistorting' the optimization landscape to produce shorter, more efficient paths in complex robotic scenarios.

Contribution

The authors develop a novel approach to handle nonconvex objectives in GCS-based motion planning by undistorting the parametrized configuration space, maintaining feasibility and improving path quality.

Findings

Significant reduction in path length and trajectory duration.

Method maintains feasibility guarantees despite nonconvexity.

Effective across diverse robotic planning domains.

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/