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)

Running example: Shortest path around an obstacle

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

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