Shortest Paths in Graphs of Convex Sets

Tobia Marcucci, Jack Umenberger, Pablo Parrilo, Russ Tedrake. Shortest Paths in Graphs of Convex Sets. On arxiv.  Submitting to TAC (hopefully this week).

Shortest Paths on Graphs of Convex Sets

Example: "Footstep planning" with $x_{n+1}=Ax_n + Bu_n$

Collision-free planning with dynamic constraints

IRIS (Fast approximate convex segmentation)

• Iteration between (large-scale) quadratic program and (relatively compact) semi-definite program (SDP)
• Scales to high dimensions, millions of obstacles
• ... enough to work on raw sensor data

• When sets $X_i$ are points, reduces to standard LP formulation of the shortest path (known to be tight).

• There are instances of this problem that are NP-hard.

• Can strengthen convex relaxation by adding additional convex constraints (implied by binary feasibility).

• Can add (piecewise-affine) dynamic constraints on pairs $(x_i,x_j).$

Remarks

Euclidean shortest path

Finding the shortest path from A to B while avoiding polygonal obstacles (“Euclidean shortest path”):

• Solvable in polytime in 2d (with a visibility graph)
• NP-hard from 3 dimensions on
• For the 3d case there exists an approximation algorithm which gives you eps-optimality in poly time
• Nothing is known for $d \ge 4$

Our approach:

• Provides polynomial-time algorithm for $d \ge 4$ that is often tight.
• Solves a more general class of problems (e.g. can add dynamic constraints).
• WIP: Certificate of optimal solutions with high probability given a suitable rounding strategy.

Should be suitable for many graph-based optimization problems

Going forward...

Example: Bipartite matching (with convex regions)

Potential Implications for Manipulation

• Could have implications for even traditional collision-free optimal motion planning?
• Major advantage is supporting (piecewise-affine) dynamic constraints
• Now exploring model-based planning through contact (physics models and perhaps deep models)
• Recent results on Warm-starting mixed-integer MPC (Marcucci and Tedrake, 2020).

The Probabilistic Roadmap (PRM)
from Choset, Howie M., et al. Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.

Summary

• New strong mixed-integer convex formulation for shortest path problems over convex regions
  • reduces to shortest path as regions become points
  • NP-hard; but strong formulation $\Rightarrow$ efficient B&B
  • Convex relaxations are often tight!  $\Rightarrow$ Rounding strategies
• Should generalize to many graph optimizations / applications

Still need

• better models/state representations
• stronger planning/control design strategies.