Motion Planning around Obstacles with Convex Optimization (2023)

Russ Tedrake

CMU Robotics Institute Seminar

Jan 27, 2023

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​

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

Two aspects of the motion planning problem:

• Guaranteed collision-free along piecewise polynomial trajectories
• Complete/globally optimal within convex decomposition
• Very efficient solutions

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)

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

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

[1,1]^T x_n \ge 1

\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

b_0 = 0.9, b_1 = 0.4

Convex relaxations provide lower bounds

Feasible solutions provide upper bounds

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

• Number of integer variables
• "Tightness" of the convex relaxation

• Motion planning transcription:

• Too many integer variables (false combinatorial complexity)
• Disjunctive programming leads to "loose" relaxations

This is the convex relaxation

(it is very loose!).

• Vertices \(V\)
• (Directed) edges \(E\)

\(\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}

• 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) \]

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

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

• 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

\varphi_{ji}

\varphi_{ik}

X_i

\ell_{i,j}(x_i, x_j) = |x_i - x_j|_2

This is the convex relaxation

(it is tight!).

• 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). \)

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

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.

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

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

• 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

• Constrain IRIS regions with geodesic convexity
• Assign a chart to each vertex
• Pullbacks define all of the natural changes of coordinates

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

​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

• ~10k regions in 3D
  
• GCS with 20k vertices and 400k edges.
  
• Online planning takes 0.3s

drake.mit.edu

• 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

drake.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​