Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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

Savva Morozov, Tobia Marcucci, Alexandre Amice, Bernhard Paus Graesdal, Rohan Bosworth, Pablo A. Parrilo, Russ Tedrake

TL;DR

This work generalizes the classical all-pairs shortest-path problem to graphs of convex sets to enable multi-query precomputation for robotics problems that combine discrete routing with continuous state trajectories. It introduces a two-stage solution: offline synthesis of cost-to-go lower bounds via semidefinite programming, and online rollout with a greedy, short-horizon lookahead policy that uses these bounds to build paths incrementally. To handle the no-vertex-revisit constraint, the authors incorporate revisit penalties and relaxations that yield tractable SDPs with quadratic lower bounds, achieving near-optimal paths with fast online solves; experiments on a 7-DoF robotic arm show substantial speedups (offline ~6s, online ~2–11 ms per query) and competitive path quality. The approach scales to large graphs and high-dimensional configuration spaces, offering practical deployment potential in robotics through offline precomputation and fast online planning.

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.

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

TL;DR

This work generalizes the classical all-pairs shortest-path problem to graphs of convex sets to enable multi-query precomputation for robotics problems that combine discrete routing with continuous state trajectories. It introduces a two-stage solution: offline synthesis of cost-to-go lower bounds via semidefinite programming, and online rollout with a greedy, short-horizon lookahead policy that uses these bounds to build paths incrementally. To handle the no-vertex-revisit constraint, the authors incorporate revisit penalties and relaxations that yield tractable SDPs with quadratic lower bounds, achieving near-optimal paths with fast online solves; experiments on a 7-DoF robotic arm show substantial speedups (offline ~6s, online ~2–11 ms per query) and competitive path quality. The approach scales to large graphs and high-dimensional configuration spaces, offering practical deployment potential in robotics through offline precomputation and fast online planning.

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.
Paper Structure (10 sections, 3 equations, 2 figures)

This paper contains 10 sections, 3 equations, 2 figures.

Table of Contents

  1. Introduction
  2. Literature review
  3. All-Pairs Shortest Paths in a Graph of Convex Sets
  4. All-Pairs Shortest Paths
  5. Graphs and paths.
  6. Shortest Path Problem (SPP).
  7. All-Pairs Shortest Paths (APSP).
  8. Shortest-Path Problem in a Graph of Convex Sets
  9. Graph of Convex Sets.
  10. The Shortest Path Problem in a Graph of Convex Sets.

Figures (2)

  • Figure 1: Robotic arm in a simulated environment, tasked with moving items between shelves and bins. Shown are four queries for collision-free motion planning.
  • Figure 2: Illustration of our approach. The GCS instance is embedded in $\mathbb R^2$, with the source vertex at the top and the target vertex at the bottom. The edges are shown as red arrows, and the edge length is the squared Euclidean distance.