Motion planning around obstacles with Graphs of Convex Sets

Russ Tedrake

 

Columbia ME Seminar

May 5, 2023

Slides available live at https://slides.com/d/548A4jE/live
or later at https://slides.com/russtedrake/2023-columbia

A golden age for robotics

https://code-as-policies.github.io/

https://code-as-policies.github.io/

​"What's still hard for AI" by Kai-Fu Lee:

  1. AI cannot create, conceptualize, or manage complex strategic planning.

  2. AI cannot accomplish complex work that requires precise hand-eye coordination.

  3. AI cannot deal with unknown and unstructured spaces, especially ones that it hasn’t observed.

  4. AI cannot, unlike humans, feel or interact with empathy and compassion; therefore, it is unlikely that humans would opt for interacting with an apathetic robot for traditional communication services.

Kai-Fu's key axes of development:

  • Manual dexterity
  • Social intelligence (empathy/compassion)

written in 2018

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

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

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

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

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

  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:

     
  3. New Graphs of Convex Sets (GCS) machinery
     
  4. 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} \in {0, 1}, && \forall (i,j) \in E. \end{aligned}

"flow constraints"

binary relaxation

path length

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

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

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

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 collision-free 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

Planning through contact

Planning through contact

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
     
  • 20k vertices and 400k edges.
     
  • Online planning takes 0.3s

by Tobia Marcucci in collaboration w/ Stephen Boyd

drake.mit.edu

Summary

  • New strong mixed-integer convex formulation for shortest path problems over convex regions
    • Immediate applications in manipulator planning at dynamic limits
  • Foundation models for control?
    • Have not yet seen zero-shot generalization in manipulation
    • Stronger planners \(\Rightarrow\) less human demonstrations

 

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​

"Hydroelastic contact" as implemented in Drake

Copy of Motion Planning around Obstacles with Graphs of Convex Sets

By Bernhard Paus Græsdal

Source: russtedrake

Copy of Motion Planning around Obstacles with Graphs of Convex Sets

Columbia Mechanical Engineering Seminar

  • 211

More from Bernhard Paus Græsdal