# Motion Planning

Part II: Sampling-based and

global optimization

MIT 6.4210/2:

Robotic Manipulation

Fall 2022, Lecture 14

Follow live at https://slides.com/d/dL0qIfo/live

(or later at https://slides.com/russtedrake/fall22-lec14)

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.

## Probabilistic Roadmaps (PRMs)

"Multi-query" planner separates:

1. Roadmap construction (offline)
2. Graph search (online)

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.

/// Generate a roadmap.
Graph<Eigen::VectorXd> BuildRoadmap(
int64_t roadmap_size, int64_t num_neighbors,
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
/// *AddingNodes versions whenever possible to avoid copies of roadmap.
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​

## Shortest Paths on Graphs of Convex Sets

+ time-rescaling

### But how did we get the convex regions?

(I thought we said C-space was complicated??)

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
• Guaranteed collision-free along piecewise polynomial trajectories
• Complete/globally optimal within convex decomposition

## Configuration-space regions

• 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

IRIS shelves demo

### The Graph of Convex Sets (GCS) motion planning framework

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

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 14: Motion Planning II

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

• 364