# Progress on Graphs of Convex Sets

(and their applications in robotics)

Russ Tedrake

Seminar at The AI Institute
June 4, 2024

## Optimal Control

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

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)

## Combinatorial: Sampling-based motion planning

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

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

+ time-rescaling

## GCS Trajectory Optimization

Preprocessor makes easy optimizations fast!

## GCS Trajectory Optimization

Transitioning from basic research to real use cases

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)

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

## Current sweet spot for multiquery planning

Goal-conditioned value functions

+

+

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

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

# Large Behavior Models

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

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​

By russtedrake

# Graphs of Convex Sets

Columbia Mechanical Engineering Seminar

• 217