Multi-Query Shortest-Path Problem in Graphs of Convex Sets

TL;DR

This paper introduces a multi-query extension of the Shortest-Path Problem in Graphs of Convex Sets, enabling faster and higher quality motion planning for robot arms by combining offline bounds with online convex optimization.

Contribution

It proposes a novel two-stage approach combining semidefinite programming and incremental convex programming for efficient multi-query motion planning.

Findings

Achieves up to two orders of magnitude faster planning times.

Provides higher quality trajectories compared to existing methods.

Enables efficient precomputation for multiple start and goal configurations.

Abstract

The Shortest-Path Problem in Graph of Convex Sets (SPP in GCS) is a recently developed optimization framework that blends discrete and continuous decision making. Many relevant problems in robotics, such as collision-free motion planning, can be cast and solved as an SPP in GCS, yielding lower-cost solutions and faster runtimes than state-of-the-art algorithms. In this paper, we are motivated by motion planning of robot arms that must operate swiftly in static environments. We consider a multi-query extension of the SPP in GCS, where the goal is to efficiently precompute optimal paths between given sets of initial and target conditions. Our solution consists of two stages. Offline, we use semidefinite programming to compute a coarse lower bound on the problem's cost-to-go function. Then, online, this lower bound is used to incrementally generate feasible paths by solving short-horizon convex programs. For a robot arm with seven joints, our method designs higher quality trajectories up to two orders of magnitude faster than existing motion planners.