# Motion Planning

Part II: Sampling-based and

global optimization

MIT 6.4210/2:

Robotic Manipulation

Fall 2022, Lecture 14

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

"Multi-query" planner separates:

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.

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​

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