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

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

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

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

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

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

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

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!

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.

  • 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

drake.mit.edu

"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

drake.mit.edu

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

Talk at UCSD, May 12, 2022

  • 1,248

More from russtedrake