russtedrake PRO
Roboticist at Walden Robotics and MIT
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
Discrete paths + continuous (convex via differential flatness)
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
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
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
