Motion Planning

 

MIT 6.4210/2

Robotic Manipulation

Fall 2026, Lecture 6

Visibility graph in Cspace  (Lozano-Perez and Wesley) 1977-99

Handey (Lozano-Perez, et al) 1987-89

Sample-based (Barraquand, Kavraki, Latombe, LaValle, et al) 1990-2000

Hardware assist - 2016

https://github.com/KavrakiLab/vamp

CUDA-Accelerated Robot Motion Generation in Milliseconds with NVIDIA cuRobo

Asymptotically Optimal Sampling Based Planners

Global optimization-based planning

Motion planning as a (nonconvex) optimization

\begin{aligned} \min_{q_0, ..., q_N} \quad & \sum_{n=0}^{N-1} | q_{n+1} - q_n|_2^2 & \\ \text{subject to} \quad & q_0 = q_{start} \\ & q_N = q_{goal} \\ & |q_n|_1 \ge 1 & \forall n \end{aligned}

start

goal

fixed number of samples

collision-avoidance

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

nonconvex

Motion Planning around Obstacles with Convex Optimization.

Tobia Marcucci, Mark Petersen, David von Wrangel, Russ Tedrake.

Available at: https://arxiv.org/abs/2205.04422​

Accepted for publication in Science Robotics

Motion planning with Graphs of Convex Sets (GCS)

Claims:

  • Find better plans faster than sampling-based planners
  • Avoid local minima from trajectory optimization
  • Can guarantee paths are collision-free
  • Naturally handles dynamic limits/constraints
  • Scales to big problems (e.g. multiple arms)

GCS Trajectory Optimization

Sampling-based motion planning

The Probabilistic Roadmap (PRM)
from Choset, Howie M., et al.
Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.

  • PRM: A roadmap of points (vertices) connected by line segments
  • GCS: A roadmap of convex sets (vertices) of continuous curves connected by continuity constraints

Key ingredients

  • (Approximate) convex decomposition of collision-free configuration space.  (aka "IRIS")
  • Efficient algorithms for solving shortest paths on "Graphs of Convex Sets" (aka "GCS")
  • Convex optimization over continuous curves w/ time scaling (\(\Rightarrow\) "GCS Trajectory Optimization")

IRIS for Configuration-space regions

  • IRIS (Fast approximate convex segmentation).  Deits and Tedrake, 2014
  • New versions work in configuration space:
    • Nonlinear optimization for speed. IRIS-NP
    • Sums-of-squares for rigorous certification. C-IRIS
q_2
q_1

Graphs of Convex Sets

Note: The blue regions are not obstacles.

Graphs of Convex Sets

Mixed-integer formulation with a very tight convex relaxation

  • Efficient branch and bound, or
  • In practice we only solve the convex relaxation and round

Main idea: Multiply constraints + Perspective function machinery

+ time-rescaling

GCS Trajectory Optimization