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