Motion planning around obstacles with convex optimization

Russ Tedrake

 

WAFR 2022

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

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 an optimization

Running example: Shortest path around an obstacle

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

fixed number of samples

collision-avoidance

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

nonconvex

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:

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

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

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

          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

Presentation

Friday @ 14:30

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

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

 

  • Starting work on a custom solver
  • GCS can warm-start well

"Hydroelastic contact" as implemented in Drake

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​

Motion Planning around Obstacles with Convex Optimization

By russtedrake

Motion Planning around Obstacles with Convex Optimization

WAFR 2022 Keynote

  • 369