# Motion Planning

Part II: Sampling-based

and global optimization

MIT 6.421

Robotic Manipulation

Fall 2023, Lecture 12

(or later at https://slides.com/russtedrake/fall23-lec12)

start

goal

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

## Rapidly-exploring random trees (RRTs)

BUILD_RRT (qinit) {
T.init(qinit);
for k = 1 to K do
qrand = RANDOM_CONFIG();
EXTEND(T, qrand)
}

Naive Sampling

RRTs have a "Voronoi-bias"

goal

start

Amato, Nancy M., and Yan Wu. "A randomized roadmap method for path and manipulation planning." Proceedings of IEEE international conference on robotics and automation. Vol. 1. IEEE, 1996.

"Multi-query" planner separates:

2. Graph search (online)
BUILD_ROADMAP () {
V = {}, E = {}
for k = 1 to K
repeat
q = RANDOM_CONFIG()
until q is collision free
V.insert(q)
for all q in V
for all qn in NearestNeighbors(q, V)
if {q,qn} is collision free
E.insert({q,qn})
}

from Choset, Howie M., et al.
Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.

from Choset, Howie M., et al.
Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.

## Open Motion Planning Library (OMPL)

Google "drake+ompl" to find some examples (on stackoverflow) of drake integration in C++.  Using the python bindings should work, too.

## Sampling-based planners coming to Drake

/// Plans a path using a bidirectional RRT from start to goal.
PlanningResult PlanBiRRTPath(
const Eigen::VectorXd& start, const Eigen::VectorXd& goal,
const BiRRTPlannerParameters& parameters,
const CollisionCheckerBase& collision_checker,
const std::unordered_set<drake::multibody::BodyIndex>& ignored_bodies,
StateSamplingInterface<Eigen::VectorXd>* sampler);

/// Bidirectional T-RRT planner.
CostPlanningResult PlanTRRTPath(
const CostPlanningStateType& start, const Eigen::VectorXd& goal,
const TRRTPlannerParameters& parameters,
const CollisionCheckerBase& collision_checker,
const std::unordered_set<drake::multibody::BodyIndex>& ignored_bodies,
const MotionCostFunction& motion_cost_function,
StateSamplingInterface<Eigen::VectorXd>* sampler);

/// Roadmap creation and updating for kinematic PRM planning.

const CollisionCheckerBase& collision_checker,
const std::unordered_set<drake::multibody::BodyIndex>& ignored_bodies,
StateSamplingInterface<Eigen::VectorXd>* sampler);

/// Plans a path through the provided roadmap. In general, you should use the
PlanningResult PlanPRMPath(
const Eigen::VectorXd& start, const Eigen::VectorXd& goal,
int64_t num_neighbors, const CollisionCheckerBase& collision_checker,
const std::unordered_set<drake::multibody::BodyIndex>& ignored_bodies,
const Graph<Eigen::VectorXd>& roadmap, bool parallelize = true);


# Global optimization-based planning

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)
\ell_{i,j}(x_i, x_j) = |x_i - x_j|_2

start

goal

# GCS Trajectory Optimization

## Sampling-based motion planning

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

## Convex decomposition of configuration space

• Insight: Cliques in the visibility graph (almost) correspond to convex sets in configuration space
• Approach: (Approximate) minimum clique cover

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

Transitioning from basic research to real use cases

\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

Graph of Convex Sets (GCS)

PRM

PRM w/ short-cutting

Preprocessor now makes easy optimizations fast!

Default playback at .25x

## Scaling

• ~10k regions in 3D

• 20k vertices and 400k edges.

• Online planning takes 0.3s

by Tobia Marcucci in collaboration w/ Stephen Boyd

PRM

GCS

## Time-optimal rescaling

work w/ Mark Petersen

Having said all that...

I don't actually love "collision-free motion planning" as a formulation...

By russtedrake

# Lecture 12: Motion Planning II

MIT Robotic Manipulation Fall 2023 http://manipulation.csail.mit.edu

• 725