Russ Tedrake

CMU Robotics Institute Seminar

Jan 27, 2023

Slides available live at https://slides.com/d/Dqc6oN4/live

or later at https://slides.com/russtedrake/2023-cmu-ri

*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

## Running example: Shortest path around an obstacle

start

goal

(e.g. over homotopy classes)*Combinatorial*optimization (over curves)*Smooth*

Two aspects of the *motion planning problem:*

*start*

*goal*

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

## Goal #1: Motion planning

Efficient algorithms for (approximate) *global *optimization-based planning for manipulators *with dynamic constraints*

image credit: James Kuffner

## Goal #2: Deeper connections between

Trajectory optimization

Sample-based planning

AI-style logical planning

Combinatorial optimization

### A preview of the results...

*Default playback at .25x*

## 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 - Very efficient solutions

## 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: - New
machinery*Graphs of Convex Sets (GCS)* - New
*approximate convex decompositions*of configuration space

**Kinematic Trajectory Optimization**

(for robot arms)

### Motion planning as an optimization

## Motion planning as a (nonconvex) optimization

\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} \\ & |x_n|_1 \ge 1 & \forall n \end{aligned}

start

goal

*fixed number of samples*

*collision-avoidance*

*(outside the \(L^1\) ball)*

*nonconvex*

## Planning as a (nonconvex) optimization

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

## Mixed-integer programs

\begin{aligned} \underset{x, b}{\text{minimize}} \quad & f(x, b) \\\text{subject to} \quad & g(x, b) \le 0 \\ & b_i \in \{0, 1\}, \forall i \end{aligned}

0 \le b_i \le 1

*"Convex relaxation"* replaces this with:

*"Mixed-integer convex"* iff \(f\) and \(g\) are convex.

Convex relaxation is *"tight"* when the relaxed solution is a solution to the original problem.

## Branch and bound

b_0 = 0.9, b_1 = 0.4

b_0 = 1

b_0 = 0

b_1 = 1

b_1 = 0

Convex relaxations provide *lower bounds*

Feasible solutions provide *upper bounds*

convex

convex

convex

convex

convex

\begin{aligned} \underset{x, b}{\text{minimize}} \quad & f(x, b) \\\text{subject to} \quad & g(x, b) \le 0 \\ & b_i \in \{0, 1\}, \forall i \end{aligned}

## 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\)

## 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} \geq 0, && \forall (i,j) \in E.\end{aligned}

*"flow constraints"*

*binary relaxation*

*path length*

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

## New 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**

## New shortest path formulation

\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}

*Non-negative scaling of a convex set is still convex (e.g. via "perspective functions")*

Achieved orders of magnitude speedups.

## Tightening the convex relaxation

- Tobia continued to study the convex relaxation on simple graphs.
- Pablo: "you're still missing a constraint"

\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

(this was the missing constraint!)

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

## Euclidean shortest path

- Finding the shortest path from A to B while avoiding polygonal obstacles (“Euclidean shortest path”):
- Solvable in polytime in 2D (with a visibility graph)
- NP-hard from 3 dimensions on
- For the 3D case there exists an approximation algorithm which gives you \(\epsilon\)-optimality in poly time
- Nothing is known for dimension \(\ge 4\)

- New formulation:
- Provides polynomial-time algorithm for dimension \(\ge 4\) that is often tight.
- Solves a more general class of problems (e.g. can add dynamic constraints).

- When sets \( X_i \) are points, reduces to standard LP formulation of the shortest path (known to be tight).

- There are instances of this problem that are NP-hard.
- We give simple examples where relaxation is not tight.

- Can add (piecewise-affine) dynamic constraints on pairs \( (x_i,x_j). \)

## Remarks

## Example: "Footstep planning" with \(x_{n+1}=Ax_n + Bu_n\)

Previous best formulations | New formulation | |
---|---|---|

Lower Bound (from convex relaxation) | 7% of MICP | 80% of MICP |

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

## Configuration-space regions

- Original IRIS algorithm assumed obstacles were convex
- Two new extensions for C-space:
- Nonlinear optimization for speed
- Sums-of-squares for rigorous certification

q_1

q_2

**WAFR 2022; **

**Journal version out this month.**

## Sums-of-Squares formulation

Solve using rational polynomial kinematics

The separating plane (green) is the non-collision certificate between the two highlighted polytopic collision geometries (red), with a distance of 7.3mm.

Graph of Convex Sets (GCS)

PRM

PRM w/ short-cutting

Preprocessor now makes easy optimizations fast!

## Geometry Problems

- We don't yet understand rigorously what defines optimal convex decompositions for this planner
- closely related to the
*maximum vertex hidden set problem*and*visibility graphs* - subtle costs/benefits of overlapping regions
- subtle trade-offs between coverage and complexity of the sets

- closely related to the

## Extensions to Non-Euclidean spaces

- Constrain IRIS regions with geodesic convexity
- Assign a chart to each vertex
- Pullbacks define all of the natural changes of coordinates

with Tommy Cohn, Mark Petersen, and Max Simchowitz

## 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 (e.g. TSP w/ neighborhoods)
- For control
- Linear quadratic regulator model-predictive control (LQR MPC)
- extends naturally to
*piecewise affine*systems

## Task and motion planning

GCS version (top down)

## Task and motion planning with GCS

Prelimary results by Savva Morozov

## 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've provided a mature implementation

There is much more to do, for example:

- Add support for additional costs / constraints
- Dynamic collision geometry / moving obstacles

- GCS can warm-start well
- Working on a custom solver with Stephen Boyd

## Scaling

- ~10k regions in 3D
- GCS with 20k vertices and 400k edges.
- Online planning takes 0.3s

by Tobia Marcucci in collaboration w/ Stephen Boyd

## 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
*manipulator planning at dynamic limits*

Give it a try:

`pip install drakesudo apt install drake`

## Goal #2: Deeper connections between

Trajectory optimization

Sample-based planning

AI-style logical planning

Combinatorial optimization

## Online classes (videos + lecture notes + code)

http://manipulation.mit.edu

http://underactuated.mit.edu

*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