Motion planning

as search II

MIT 6.832: Underactuated Robotics

Spring 2022, Lecture 20

Follow live at https://slides.com/d/ubLxnxU/live
(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.

Shortest Paths on Graphs of Convex Sets

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

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