russtedrake PRO
Roboticist at MIT and TRI
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:
- Discrete: Plan contact sequence (e.g. footsteps)
- 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
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
- Combinatorial (e.g. over homotopy classes)
- 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
For a living doc with up-to-date references / examples:
Graphs of Convex Sets
By russtedrake
Graphs of Convex Sets
RSS 2024 workshop on Frontiers of optimization for robotics https://sites.google.com/robotics.utias.utoronto.ca/frontiers-optimization-rss24/home
