Progress on Graphs of Convex Sets

(and their applications in robotics)

Russ Tedrake

Seminar at The AI Institute
June 4, 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

sequential composition

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

  • 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) \]
  • New efficient algorithms based on tight convex relaxations

Network flow 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

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...

  • 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)  

Maybe ask me about RL during the Q/A?

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

As a motion planning tool

Near-term-goal: complete planning solution for mobile manipulation on Spot

(collaborating w/ Leslie Kaelbling and Tomas Lozano-Perez)

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
  • Learning and Planning should work in harmony

 

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

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

Available at: https://arxiv.org/abs/2101.11565

Motion Planning around Obstacles with Convex Optimization.

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

Available at: https://arxiv.org/abs/2205.04422​

Graphs of Convex Sets

By russtedrake

Graphs of Convex Sets

Columbia Mechanical Engineering Seminar

  • 361

More from russtedrake