Motion Planning around Obstacles with Convex Optimization (2023)

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

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

from Choset, Howie M., et al.
Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.

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

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 decompositionsof configuration space

Kinematic Trajectory Optimization

(for robot arms)

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}

(Video) Russ Tedrake: Motion Planning Around Obstacles with Convex Optimization

start

goal

fixed number of samples

collision-avoidance

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

nonconvex

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. 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 (Video) Invited talk: Motion Planning around Obstacles with Convex Optimization by Tobia Marcucci, MIT 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 exactlyare 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

(Video) RI Seminar: Russ Tedrake : Motion Planning Around Obstacles with Graphs of Convex Sets

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

Example: "Footstep planning" with $$x_{n+1}=Ax_n + Bu_n$$

Previous best formulationsNew formulation
Lower Bound
(from convex relaxation)
7% of MICP80% 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

(Video) Shortest Paths in Graphs of Convex Sets, with Applications to Control and Motion Planning

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

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:

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

(Video) Real Time Quad Rotor Path Planning for Mobile Obstacle Avoidance Using Convex Optimization

Videos

1. Autonomy Talks - Kristoffer Bergman: Sampling-​Based Motion Planning and Direct Optimal Control
(ETH Zürich Frazzoli)
2. Lecture 15: MIT 6.800/6.843 Robotics Manipulation (Fall 2021) | "Motion Planning Optimization-based"
(underactuated)
3. Self-Driving Cars - Lecture 12.4 (Decision Making and Planning: Motion Planning)
(Tübingen Machine Learning)
4. Lecture 20: Optimization in Motion Planning
(IIT Kanpur July 2018)
5. GuSTO: Guaranteed Sequential Trajectory Optimization via Sequential Convex Programming
(Stanford Autonomous Systems Laboratory)
6. Motion planning - one obstacle
(Amir Yazdani)
Top Articles
Latest Posts
Article information

Author: Amb. Frankie Simonis

Last Updated: 03/09/2023

Views: 5481

Rating: 4.6 / 5 (76 voted)

Author information

Name: Amb. Frankie Simonis

Birthday: 1998-02-19

Address: 64841 Delmar Isle, North Wiley, OR 74073

Phone: +17844167847676

Job: Forward IT Agent

Hobby: LARPing, Kitesurfing, Sewing, Digital arts, Sand art, Gardening, Dance

Introduction: My name is Amb. Frankie Simonis, I am a hilarious, enchanting, energetic, cooperative, innocent, cute, joyous person who loves writing and wants to share my knowledge and understanding with you.