Graphs of Convex Sets

(and their applications in robotics)

Russ Tedrake

RSS24 - Optimization Workshop
July 15, 2024

Optimal Control

\begin{align*} \min_{x[\cdot], u[\cdot]}\quad& \sum_{n=0}^N \ell(x[n], u[n])\\ \text{s.t.}\quad& x[n+1] = f(x[n],u[n])\\ & \text{additional constraints} \end{align*}

There are (only?) two cases that we completely understand:

  • Tabular/Markdov Decision Process (discrete state & action),
  • Linear+Convex (e.g. LQR, linear MPC).

Planning and control through contact

Two stages:

  1. Discrete: Plan contact sequence (e.g. footsteps)
  2. Continuous: Nonlinear MPC

Task and motion planning

Integrate:

  • Discrete: Large-scale discrete planners (e.g. Fast downward)
  • Continuous: e.g. Sampling-based planners

start

goal

  • Discrete/Combinatorial (e.g. over homotopy classes)
  • Continuous/Smooth optimization (over curves)

(even) Collision-free motion planning

Combinatorial: Sampling-based motion planning

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

Smooth: Trajectory Optimization

Graphs of convex sets (GCS) offers a new perspective on joint discrete + continuous optimization

Shortest Paths in Graphs of Convex Sets.
Tobia Marcucci, Jack Umenberger, Pablo Parrilo, Russ Tedrake.

SIAM Journal on Optimization, 2024

Motion Planning around Obstacles with Convex Optimization.

Tobia Marcucci, Mark Petersen, David von Wrangel, Russ Tedrake.

Science Robotics, 2023

Graphs of Convex Sets with Applications to Optimal Control and Motion Planning.

Tobia Marcucci. PhD Thesis, MIT, 2024

Traditional Shortest Path as a Linear Program (LP)

\(\varphi_{ij} = 1\) if the edge \((i,j)\) in shortest path, otherwise \(\varphi_{ij} = 0.\)

\(c_{ij} \) is the (constant) length of edge \((i,j).\)

\begin{aligned} \min_{\varphi} \quad & \sum_{(i,j) \in E} c_{ij} \varphi_{ij} \\ \mathrm{s.t.} \quad & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si}, && \forall i \in V, \\ & \varphi_{ij} \in \{0, 1\}, && \forall (i,j) \in E. \end{aligned}

"flow constraints"

binary relaxation

path length

\begin{aligned} \min_{\varphi} \quad & \sum_{(i,j) \in E} c_{ij} \varphi_{ij} \\ \mathrm{s.t.} \quad & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si}, && \forall i \in V, \\ & \varphi_{ij} \ge 0, && \forall (i,j) \in E. \end{aligned}

Graphs of Convex Sets

 

  • For each \(i \in V:\)
    • Compact convex set \(X_i \subset \R^d\)
    • A point \(x_i \in X_i \) 
  • Edge length given by a convex function \[ \ell(x_i, x_j) \]

Note: The blue regions are not obstacles.

GCS shortest path formulation

\begin{aligned} \min_{\varphi} \quad & \sum_{(i,j) \in E} c_{ij} \varphi_{ij} \\ \mathrm{s.t.} \quad & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si}, && \forall i \in V, \\ & \varphi_{ij} \geq 0, && \forall (i,j) \in E. \end{aligned}

Classic shortest path LP

\begin{aligned} \min_{\varphi,x} \quad & \sum_{(i,j) \in E} \ell_{ij}(x_i, x_j) \varphi_{ij} \\ \mathrm{s.t.} \quad & x_i \in X_i, && \forall i \in V, \\ & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si} \le 1, && \forall i \in V, \\ & \varphi_{ij} \geq 0, && \forall (i,j) \in E. \end{aligned}

now w/ Convex Sets

GCS "Machinery"

\begin{aligned} \min_{\varphi,x} \quad & \sum_{(i,j) \in E} \ell_{ij}(x_i, x_j) \varphi_{ij} \\ \mathrm{s.t.} \quad & x_i \in X_i, && \forall i \in V, \\ & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si} \le 1, && \forall i \in V, \\ & \varphi_{ij} \geq 0, && \forall (i,j) \in E. \end{aligned}
  • Transcribe bilinear form into MI-convex
  • Multiply discrete constraints w/ continuous constraints for efficient and often tight convex relaxation 

Traveling Salesperson

\begin{aligned} \min_{\varphi} \quad & \sum_{(i,j) \in E} c_{ij} \varphi_{ij} \\ \mathrm{s.t.} \quad & \sum_{j \in E_i^{out}} \varphi_{ij} = \sum_{j \in E_i^{in}} \varphi_{ji} = 1, && \forall i \in V, \\ & \varphi_{ij} \geq 0, && \forall (i,j) \in E. \end{aligned}
  • famously NP-hard
  • apply GCS machinery for efficient MIP generalization
  • orders of magnitude faster than previous "TSP w/ neighborhood" formulations

Graph optimization problems

Shortest path, Traveling salesperson, minimum spanning tree, bipartite matching, facility location, ...

Ex: Minimum spanning tree

Ex: Minimum-volume sphere collision geometry (as facility location on a GCS)

Tabular + Linear MPC

Finite MDP (e.g. w/ deterministic transitions) is shortest path

When sets are points, GCS transcription yields exactly the well-known linear program (LP) for shorts path.

Tabular + Linear MPC

\[ \min_{x[\cdot],u[\cdot]} \sum_{n=0}^N x_n^T Q x_n + u_n^T Ru_n \\ \text{s.t. } x_{n+1} = Ax_n + Bu_n \]

Sets \( X_n: (x_n, u_n) \)

Edge cost

Edge constraint

n=0

n=1

n=2

n=N

\( \cdots \)

For a serial chain, GCS will generate exactly the familiar MPC transcriptions, e.g. quadratic programs (QPs)

Mixed logical dynamical systems (MLDS)

e.g. for hybrid trajectory optimization

n=0

n=1

n=N

...

\[ \min_{x[\cdot],u[\cdot]} \sum_{n=0}^N x_n^T Q_i x_n + u_n^T R_iu_n \\ \text{s.t. } x_{n+1} = A_ix_n + B_iu_n \\ \text{iff } (x_n,u_n) \in D_i \] 

start

goal

          is the convex relaxation.  (it's tight!)

Previous formulations were intractable; would have required \( 6.25 \times 10^6\) binaries.

minimum distance

minimum time

GCS Trajectory Optimization

Convex optimization problems as "Convex Sets"

  • For GcsTrajOpt, each convex set is a mathematical program
    • ​(convex) kinematic trajectory optimization
    • Bezier curves + time scaling

 

Workshop poster today on tight SDP relaxations of time-scaling + linear dynamics

A preview of the results...

Discrete paths + continuous (convex via differential flatness)

GCS Trajectory Optimization

Sampling-based motion planning

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

  • PRM: A roadmap of points (vertices) connected by line segments
  • GCSTrajOpt: A roadmap of convex sets (vertices) of continuous curves connected by continuity constraints

Key ingredients

  • Efficient algorithms for solving shortest paths on "Graphs of Convex Sets" (aka "GCS")
  • Transcription of the trajectory optimization problem into GCS
    • (Approximate) convex decomposition of collision-free configuration space.  (aka "IRIS")
    • Convex optimization over continuous curves w/ time scaling (\(\Rightarrow\) "GCS Trajectory Optimization")

IRIS for Configuration-space regions

  • IRIS (Fast approximate convex segmentation).  Deits and Tedrake, 2014
  • New versions work in configuration space:
    • Nonlinear optimization for speed. IRIS-NP
    • Sums-of-squares for rigorous certification. C-IRIS
q_2
q_1

Convex decomposition of configuration space

  • Naive sampling can lead to inefficient coverage
  • New algorithms optimize a convex cover
    • Insight: Cliques in the visibility graph (almost) correspond to convex sets in configuration space
    • Approach: (Approximate) minimum clique cover

 

 

(Approximate) Minimum clique cover

+ time-rescaling

GCS Trajectory Optimization

Preprocessor makes easy optimizations fast!

GCS Trajectory Optimization

Transitioning from basic research to real use cases

Adoption

Adoption

Dave Johnson (CEO): "wow -- gcs (left) is a LOT better! ... This is a pretty special upgrade which is going to become the gold standard for motion planning."

Bernhard Paus Graesdal

Planning Through Contact with GCS

Going beyond collision-free motion planning...

Friday morning session

  • Plan with quasi-static dynamics
    • \( v \approx 0\) and \(M \dot v + C v \approx 0 \)
    • No impacts
  • Plan over object poses, contact forces and contact locations
    \( \implies \) Nonlinear kinematics & dynamics

Formulation

Using GCS for Planning Through Contact

  • Our quasi-static dynamics and kinematic constraints form a nonconvex QCQP
  • Semidefinite relaxation of QCQP (with some new additions)
  • Feasible set is a Spectrahedron (a convex set)
\begin{aligned} &\tau_g(q) + \sum_i J_{c,i}^\intercal f^{c_i} = 0 \end{aligned}
{}^A R^B \in SO(2)
f^{c_i} \in \mathcal{FC}_i

Nonconvex dynamics and kinematics

  • The convex relaxation doesn't have to be perfect
  • \( \implies \) Only needs to contain enough information to take the correct high-level decisions
  • Then we can refine the details as a final step!

or

\(\longrightarrow\)

\(\longrightarrow\)

\(\longrightarrow\)

\(\longrightarrow\)

\(\longrightarrow\)

\(\longrightarrow\)

\(\longrightarrow\)

Start

Goal

Planning through contact for Planar Pushing

start

goal

  1. Combinatorial (e.g. over homotopy classes)
  2. Smooth optimization (over curves)

Two aspects of the motion planning problem:

Lots of work on "contact graphs", but it's a little more subtle than that...

An aside

Stronger optimization enables simpler cost functions.

 

(No cost-function tuning required)  

These algorithms are not arbitrary.

 

GCS helped us see the deeper connections between motion planning and structured optimization (SDP relaxations, moment hierarchies, etc).

  • Stochastic/robust MPC
  • Planning under uncertainty
  • ...

Value functions on GCS

(e.g. for fast multiquery planning)

What if:

  • A single (slightly bigger) optimization offline, which solves for all initial conditions

So far:

  • Solve an optimization for each initial condition

Lower bounds on the value function

Current sweet spot for multiquery planning

Goal-conditioned value functions

+

Piecewise-quadratic lower bounds

+

Few-step lookahead

As a motion planning tool

​This is version 0.1 of a new framework.

  • Already competitive (better paths faster; higher DOF; supports differential constraints)
  • We're providing a mature implementation in Drake

 

There is much more to do...

  • Adding support for additional costs / constraints
    • nonconvex derivative constraints, torque limits, dynamics
    • ...
  • Dynamic collision geometry / moving obstacles
  • Output feedback / information gathering

Diffusion Policies &

Large Behavior Models

but... what about 

The AlphaGo Playbook

  • Step 1: Behavior Cloning
    • from human expert games
  • Step 2: Self-play
    • Policy network
    • Value network
    • Monte Carlo tree search (MCTS)

Learning and Planning

  • Two aspects of "intelligence"; either alone seems insufficient
  • Learning guides the planning (explore high-scoring actions)
  • Planning speeds up the learning
  • Planning immediately strengthens the policy -- potentially even when we move to "zero-shot" in "open world" domains.

GCS \(\Rightarrow\) Diffusion Policy

GCS generalization of Monte Carlo tree search

from A Survey of Monte Carlo Tree Search Methods by Browne et at, 2012

MCTS

Summary

  • GCS modeling jointly optimizes:
    • combinatorial/discrete
    • smooth/continuous
  • Strongest planners I have had; often solves to global optimality
    • and plenty more headroom
  • Not just computational; also a conceptual framework
  • Not just for motion planning

 

Give it a try:

pip install drake
sudo apt install drake

drake.mit.edu

For a living doc with up-to-date references / examples:

https://underactuated.mit.edu/optimization.html#gcs