russtedrake PRO
Roboticist at MIT and TRI
Motion planning around obstacles with convex optimization
Russ Tedrake
representing the team with Asu Ozdaglar and Costis Daskalakis
Slides available live at https://slides.com/d/UzM9m5A/live
or later at https://slides.com/russtedrake/2022-11-02-dsta-seminar
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
Goal #1: Motion planning/Model-Predictive Control
Global optimization-based planning for vehicles and manipulators with dynamic constraints and certificates of optimality / correctness
image credit: James Kuffner
Goal #2: Deeper connections between
Trajectory optimization
Sample-based planning
AI-style logical planning
Combinatorial optimization
Motion planning with obstacles as an optimization
Running example: Shortest path around an obstacle
start
goal
Planning as a nonconvex optimization
A preview of the results...
Default playback at .25x
Two key ingredients
- The linear programming formulation of the shortest path problem on a discrete graph.
- Convex formulations of continuous motion planning (without obstacle navigation), for example:
Kinematic Trajectory Optimization
(for robot arms)
Planning as a mixed-integer convex program
\begin{aligned} \min_{x_0, ..., x_N} \quad & \sum_{n=0}^{N-1} | x_{n+1} - x_n|_2^2 & \\
\text{subject to} \quad & x_0 = x_{start} \\ & x_N = x_{goal} \end{aligned}
[1,1]^T x_n \ge 1 \quad \textbf{or} \quad
[1,1]^T x_n \le -1 \quad \textbf{or} \\
[1,-1]^T x_n \ge 1 \quad \textbf{or} \quad
[1,-1]^T x_n \le -1, \quad \forall n.
goal
start
[1,1]
disjunctive
constraints
[1,1]^T x_n \ge 1
Planning as a mixed-integer convex program
Branch and bound performance
- Number of integer variables
- "Tightness" of the convex relaxation
- Motion planning transcription:
\(\Rightarrow\) Long solve times.
b_0 = 0.9, b_1 = 0.4
b_0 = 1
b_0 = 0
b_1 = 1
b_1 = 0
- Too many integer variables (false combinatorial complexity)
- Disjunctive programming leads to "loose" relaxations
Planning as a mixed-integer convex program
This is the convex relaxation
(it is very loose!).
Pablo asked the right question...
"We know that the LP formulation of the shortest path problem is tight. Why exactly are your relaxations so loose?"
- Vertices \(V\)
- (Directed) edges \(E\)
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.
Tightening the convex relaxation
\sum_{j \in E_i^{in}} \varphi_{ji} = \sum_{k \in E_i^{out}} \varphi_{ik}
\sum_{j \in E_i^{in}} \varphi_{ji}x_i = \sum_{k \in E_i^{out}} \varphi_{ik}x_i
Conservation of flow
Spatial conservation of flow
\varphi_{ji}
\varphi_{ik}
X_i
Motion planning with Graph of Convex Sets (GCS)
\ell_{i,j}(x_i, x_j) = |x_i - x_j|_2
start
goal
Motion planning with Graph of Convex Sets (GCS)
This is the convex relaxation
(it is tight!).
is the convex relaxation. (it's tight!)
Previous formulations were intractable; would have required \( 6.25 \times 10^6\) binaries.
Formulating motion planning with differential constraints as a Graph of Convex Sets (GCS)
+ time-rescaling
\begin{aligned}
\min \quad & a T + b \int_0^T |\dot{q}(t)|_2 \,dt + c \int_0^T |\dot{q}(t)|_2^2 \,dt \\
\text{s.t.} \quad
& q \in \mathcal{C}^\eta, \\
& q(t) \in \bigcup_{i \in \mathcal{I}} \mathcal{Q}_i, && \forall t \in [0,T], \\
& \dot q(t) \in \mathcal{D}, && \forall t \in [0,T], \\
& T \in [T_{min}, T_{max}], \\
& q(0) = q_0, \ q(T) = q_T, \\
& \dot q(0) = \dot q_0, \ \dot q(T) = \dot q_T.
\end{aligned}
duration
path length
path "energy"
note: not just at samples
continuous derivatives
collision avoidance
velocity constraints
minimum distance
minimum time
Transcription to a mixed-integer convex program, but with a very tight convex relaxation.
- Solve to global optimality w/ branch & bound orders of magnitude faster than previous work
- Solving only the convex optimization (+rounding) is almost always sufficient to obtain the globally optimal solution.
But how did we get the convex regions?
IRIS (Fast approximate convex segmentation). Deits and Tedrake, 2014
- Iteration between (large-scale) quadratic program and (relatively compact) semi-definite program (SDP)
- Scales to high dimensions, millions of obstacles
- ... enough to work on raw sensor data
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.
- Guaranteed collision-free along piecewise polynomial trajectories
- Complete/globally optimal within convex decomposition
Graph of Convex Sets (GCS)
PRM
PRM w/ short-cutting
Preprocessor now makes easy optimizations fast!
Beyond motion planning
I've focused today on Graphs of convex sets (GCS) for motion planning
GCS is a more general modeling framework
- already orders of magnitude faster for a number of mixed-combinatorial continuous problems
As a motion planning tool
This is version 0.1 of a new framework.
- Already competitive
- We've provided a mature implementation
There is much more to do, for example:
- Add support for additional costs / constraints
- Dynamic collision geometry / moving obstacles
- Dual solution gives a cost-to-go; robustness to disturbances
- Work now on custom solver (with Pablo and Stephen Boyd)
- GCS can warm-start well
Summary
- New strong mixed-integer convex formulation for shortest path problems over convex regions
- reduces to shortest path as regions become points
- NP-hard; but strong formulation \(\Rightarrow\) efficient B&B
- Convex relaxations are often tight! \(\Rightarrow\) Rounding strategies
- Initial applications in vehicles and manipulator planning at dynamic limits
Give it a try:
pip install drake
sudo apt install drake
Goal #2: Deeper connections between
Trajectory optimization
Sample-based planning
AI-style logical planning
Combinatorial optimization
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
MIT-DSTA research seminar
By russtedrake
