# as search II

MIT 6.832: Underactuated Robotics

Spring 2022, Lecture 20

(or later at https://slides.com/russtedrake/spring22-lec20)

Image credit: Boston Dynamics

# Motion Planning around Obstacles with Convex Optimization

Tobia Marcucci*, Mark Petersen*, David von Wrangel, Russ Tedrake*. To appear on arxiv any day now.

## 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 convex hull reformulation + perspective functions to rewrite this as mixed-integer convex.
• Strengthen convex relaxation by adding additional convex constraints (implied at binary feasibility).

minimum distance

minimum time

IRIS (Fast approximate convex segmentation)

• 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

Graph of Convex Sets (GCS)

PRM

PRM w/ short-cutting

By russtedrake

# Lecture 20: Motion planning as search II

MIT Underactuated Robotics Spring 2021 http://underactuated.csail.mit.edu

• 2,663