Motion Planning

Part II: Sampling-based

MIT 6.800/6.843:

Robotic Manipulation

Fall 2021, Lecture 16

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

(or later at https://slides.com/russtedrake/fall21-lec16)

Inverse kinematics as an optimization

\min_q | q-q_{desired}|

subject to:

  • rich end-effector constraints
  • joint limits
  • collision avoidance
  • "gaze constraints"
  • "feet stay put"
  • balance (center of mass)
  • ...

Kinematic trajectory optimization

http://www.kuffner.org/james/plan

Rapidly-exploring random trees (RRTs)

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

http://www.kuffner.org/james/plan

Naive Sampling

RRTs have a "Voronoi-bias"

Probabilistic Roadmap (PRM)

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.

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.

Kinematic planners in Drake

Kinematic planners in Drake

/**
 * Optimizes the position trajectory of a multibody model. The trajectory is
 * represented as a B-form spline.
 */
class KinematicTrajectoryOptimization {
 public:
  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(KinematicTrajectoryOptimization);

  /// Constructs an optimization problem for a `num_positions`-element position
  /// trajectory represented as a `spline_order`-order B-form spline with
  /// `num_control_points` control_points. The initial guess used in solving the
  /// optimization problem will be the zero-trajectory.
  KinematicTrajectoryOptimization(int num_positions, int num_control_points,
                                  int spline_order = 4);

Kinematic planners in 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);

Shortest Paths in Graphs of Convex Sets

Tobia Marcucci, Jack Umenberger, Pablo Parrilo, Russ Tedrake. Shortest Paths in Graphs of Convex Sets. (Under review)

Available at: https://arxiv.org/abs/2101.11565

Shortest Paths on Graphs of Convex Sets

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

We were never quite happy...

  • Too many integer variables
    • transcriptions add "false" combinatorial complexity

 

  • Branch and bound working too hard
    • loose convex relaxations

Pablo asked the right question...

"We know that the LP formulation of the shortest path problem is tight.  Why exactly are your relaxations loose?"

  • Vertices \(V\)
  • (Directed) edges \(E\)

Traditional Shortest Path as a Linear Program (LP)

\(\varphi_{ij} = 1\) if the edge \((i,j)\) in shortest path, otherwise \(\varphi_{ij} = 0.\)

\(c_{ij} \) is the (constant) length of edge \((i,j).\)

\begin{aligned} \min_{\varphi} \quad & \sum_{(i,j) \in E} c_{ij} \varphi_{ij} \\ \mathrm{s.t.} \quad & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si}, && \forall i \in V, \\ & \varphi_{ij} \geq 0, && \forall (i,j) \in E. \end{aligned}

"flow constraints"

binary relaxation

path length

Graphs of Convex Sets

 

  • For each \(i \in V:\)
    • Compact convex set \(X_i \subset \R^d\)
    • A point \(x_i \in X_i \) 
  • Edge length given by a convex function \[ \ell(x_i, x_j) \]

New shortest path formulation

\begin{aligned} \min_{\varphi} \quad & \sum_{(i,j) \in E} c_{ij} \varphi_{ij} \\ \mathrm{s.t.} \quad & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si}, && \forall i \in V, \\ & \varphi_{ij} \geq 0, && \forall (i,j) \in E. \end{aligned}

Classic shortest path LP

\begin{aligned} \min_{\varphi,x} \quad & \sum_{(i,j) \in E} \ell_{ij}(x_i, x_j) \varphi_{ij} \\ \mathrm{s.t.} \quad & x_i \in X_i, && \forall i \in V, \\ & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si} \le 1, && \forall i \in V, \\ & \varphi_{ij} \geq 0, && \forall (i,j) \in E. \end{aligned}

now w/ Convex Sets

New shortest path formulation

\begin{aligned} \min_{\varphi,x} \quad & \sum_{(i,j) \in E} \ell_{ij}(x_i, x_j) \varphi_{ij} \\ \mathrm{s.t.} \quad & x_i \in X_i, && \forall i \in V, \\ & \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si} \le 1, && \forall i \in V, \\ & \varphi_{ij} \geq 0, && \forall (i,j) \in E. \end{aligned}
  • Use convex hull reformulation + perspective functions to rewrite this as mixed-integer convex.
  • Strengthen convex relaxation by adding additional convex constraints (implied at binary feasibility).

Running example: Shortest path around an obstacle

start

goal

Step 3:

New formulation

Did we resolve our issues?

  • Too many integer variables
    • transcriptions add "false" combinatorial complexity

 

  • Branch and bound working too hard
    • loose convex relaxations

Note: Path length is no longer predetermined

          is the convex relaxation.  (it's tight!)

Euclidean shortest path

  • Finding the shortest path from A to B while avoiding polygonal obstacles (“Euclidean shortest path”):
    • Solvable in polytime in 2D (with a visibility graph)
    • NP-hard from 3 dimensions on
    • For the 3D case there exists an approximation algorithm which gives you \(\epsilon\)-optimality in poly time
    • Nothing is known for dimension \(\ge 4\)

 

  • New formulation:
    • Provides polynomial-time algorithm for dimension \(\ge 4\) that is often tight.
    • Solves a more general class of problems (e.g. can add dynamic constraints).
  • When sets \( X_i \) are points, reduces to standard LP formulation of the shortest path (known to be tight).

 

  • There are instances of this problem that are NP-hard.
  • We give simple examples where relaxation is not tight.

  • Can add (piecewise-affine) dynamic constraints on pairs \( (x_i,x_j). \)

Remarks

Example: "Footstep planning" with \(x_{n+1}=Ax_n + Bu_n\)

Previous best formulations New formulation
Lower Bound
(from convex relaxation)
7% of MICP 80% of MICP

Most important changes (vs previous formulation)

  1. Binaries per edge, instead of per region
  2. Additional constraints which tighten the convex relaxation

Manipulators at the dynamic limits

  • Real applications are bumping up against dynamic limits (joint speed/acceleration/torque and friction limits, etc)
  • Observation: diverse motions, but relatively consistent environment.
    • Precompute regions to make online optimization fast

Trajectories in graphs of convex sets

  • B-spline parameterization
  • Convex sets are Cartesian power of C-space regions

 

  • Linear constraints connect segments
  • Linear constraints for derivative limits (joint velocity, acceleration, ...)

work w/ Andres Valenzuela

Configuration-space regions

  • Original IRIS algorithm assumed obstacles were convex
  • New extensions for C-space
    • Nonlinear optimization for speed
    • Interval arithmetic for verification

work w/ Soonho Kong

Time-optimal rescaling

work w/ Mark Petersen

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

Lecture 16: Motion Planning II

By russtedrake

Lecture 16: Motion Planning II

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

  • 977