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
start
goal
fixed number of samples
collision-avoidance
(outside the \(L^1\) ball)
nonconvex
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:
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.
Google "drake+ompl" to find some examples (on stackoverflow) of drake integration in C++. Using the python bindings should work, too.
/// 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);
Motion Planning around Obstacles with Convex Optimization.
Tobia Marcucci, Mark Petersen, David von Wrangel, Russ Tedrake.
Available at: https://arxiv.org/abs/2205.04422
+ time-rescaling
(I thought we said C-space was complicated??)
IRIS (Fast approximate convex segmentation)
IRIS shelves demo
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
work w/ Mark Petersen
Having said all that...
I don't actually love "collision-free motion planning" as a formulation...