# Motion planning around obstacles with convex optimization

Russ Tedrake

UCSD Contextual Robotics Seminar

May 12, 2022

Slides available live at https://slides.com/d/lwXONT0/live
or later at https://slides.com/russtedrake/2022-ucsd

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

Global optimization-based planning for manipulators with dynamic constraints

• Planning through contact
• Today: Collision-free planning

image credit: James Kuffner

## Goal #2: Deeper connections between

Trajectory optimization

Sample-based planning

AI-style logical planning

Combinatorial optimization

start

goal

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

nonconvex

fixed number of samples

### A preview of the results...

Default playback at .25x

## Two key ingredients

1. The linear programming formulation of the shortest path problem on a discrete graph.

2. Convex formulations of continuous motion planning (without obstacle navigation), for example:

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

start

goal

disjunctive

constraints

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

Disjunctive programming leads to "loose" relaxations.

convex relaxation replaces this with:

## Still not happy...

• Too many integer variables
• transcriptions add "false" combinatorial complexity

• Branch and bound working too hard
• loose convex relaxations

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

## 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}
• Use perspective functions to rewrite this as mixed-integer convex.
• Strengthen convex relaxation by adding additional convex constraints (implied at binary feasibility).

## Motion planning with Graph of Convex Sets (GCS)

\ell_{i,j}(x_u, x_y) = |x_u - x_v|_2

start

goal

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).$$

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

## Scaling

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
• New extensions for C-space
• Nonlinear optimization for speed
• Sums-of-squares for rigorous certification

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

## Sampling-based motion planning

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!

## Did we resolve our issues?

• Too many integer variables
• transcriptions add "false" combinatorial complexity

• Branch and bound working too hard
• loose convex relaxations

Note: Path length is no longer predetermined

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

• We've provided a mature implementation

There is much more to do, for example:

• Dynamic collision geometry / moving obstacles

• Starting work on a custom solver
• 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 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

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

By russtedrake

# Motion Planning around Obstacles with Convex Optimization

Talk at UCSD, May 12, 2022

• 814