Tree growing motion planners + optimizing planners Probabilistic Roadmaps What if only a small portion of the space needs to be explored? Tree-growing planners Idea: grow a tree of feasible paths from the start until it reaches a neighborhood of the goal

Sampling bias toward boundary of currently explored region, helps especially if the start is in a region with poor visibility Many variants: Rapidly-exploring Random Trees (RRTs) are popular, easy to implement (LaValle and Kuffner 2001) Expansive space trees (ESTs) use a different sampling strategy (Hsu et al 2001) SBL (Single-query, Bidirectional, Lazy planner) often efficient in practice (Sanchez-Ante and Latombe, 2005) RRT Build a tree T of configurations, starting at xstart Extend:

Sample a configuration xrand from C at random Find the node xnear in T that is closest to xrand Extend a short path from xnear toward xrand T RRT Build a tree T of configurations, starting at xstart Extend: Sample a configuration xrand from C at random Find the node xnear in T that is closest to xrand Extend a short path from xnear toward xrand xnear

xrand RRT Build a tree T of configurations, starting at xstart Extend: Sample a configuration xrand from C at random Find the node xnear in T that is closest to xrand Extend a short path from xnear toward xrand xnear xrand RRT Build a tree T of configurations, starting at xstart

Extend: Sample a configuration xrand from C at random Find the node xnear in T that is closest to xrand Extend a short path from xnear toward xrand xnear xrand RRT Build a tree T of configurations, starting at xstart Extend: Sample a configuration xrand from C at random Find the node xnear in T that is closest to xrand

Extend a short path from xnear toward xrand RRT Build a tree T of configurations, starting at xstart Extend: Sample a configuration xrand from C at random Find the node xnear in T that is closest to xrand Extend a short path from xnear toward xrand xnear xrand RRT

Build a tree T of configurations, starting at xstart Extend: Sample a configuration xrand from C at random Find the node xnear in T that is closest to xrand Extend a short path from xnear toward xrand RRT Build a tree T of configurations, starting at xstart Extend: Sample a configuration xrand from C at random Find the node xnear in T that is closest to xrand Extend a short path from xnear toward xrand Governed by a step size parameter d

What is the sampling strategy? Probability that a node gets selected for expansion is proportional to the volume of its Voronoi cell Implementation Details Bottleneck: finding nearest neighbor each step O(n) with nave implementation => O(n2) overall KD tree data structure O(n1-1/d) Approximate nearest neighbors often very effective Terminate when extension reaches a goal set Usually visibility set of goal configuration Bidirectional strategy

Grow a tree from goal as well as start, connect when closest nodes in either tree can see each other Optimizing planners Feasible planners: only care about first feasible solution Optimizing planners: attempt to produce a feasible path that optimizes some objective function Does the shortest path in a PRM approach the shortest path in the underlying space as more milestones are added? Theoretical results [Karaman and Frazzoli 2010] PRM:

Yes, if all neighbors within radius R are connected but O(N2) running time No, if k-nearest neighbors are connected Yes, if neighbors within radius R(N)=C (log N / N) 1/d are connected with C a volumetric constant and dim(C-space)=d Yes, if (c ln N)-nearest neighbors are connected, where N is the number of milestones and c=e*(1+1/d) RRT: No. The tree is history dependent Variant RRT*: performs tree rewiring among R(N) radius or (c ln N)-neighbors RRT

RRT* Is this the most efficient method? No! an excessive number of samples is typically needed to improve an existing path Some Theory Assume there is an -optimal shortest path with clearance from obstacles

Binary collision checks are the only information available to planner about shape of obstacles Some Theory Assume there is an -optimal shortest path with clearance from obstacles Binary collision checks are the only information available to planner about shape of obstacles Filled?

Or free? Conclude For this 2D example, there exists a set of volume that the planner must check to find a path within of optimal In -dimensions, the set has volume With uniform sampling, the number of checks needed to find this set is O() Worst case: no better than grid search! Dimensionality matters! Visibility doesnt Sampling strategy: check

configurations where they help find a better path Optimization heuristics Local postprocessing: plan a path using some PRM method, then improve it in postprocessing Shortcutting Pick two random points, test visibility. If visible, replace intervening section of path. Repeat Trajectory optimization Global strategies: Random restarts

Using a fast feasible method (e.g., RRT), repeat many times with new random seed each time. Return best path found Lazy collision checking Avoid visibility checking except for edges that could improve current best path. Reduce per-sample overhead of collision checking Motion Planning in Klampt Kris Hauser ECE 383 / ME 442 Agenda Motion planning in Klampt

Key concepts CSpace Feasibility checker (static collision checking) Visibility checker (dynamic collision checking) MotionPlan Klampt Python API Toolkit Components (today)

Modeling Planning 3D math & geometry Paths & trajectories Inverse Kinematics Motion

planning Dynamics Forward Kinematics Path smoothing Trajectory optimization Contact

mechanics Physics simulation Design tools Visualization System integration ROS interface Disk I/O JSON /

Sockets Robot posing Motion design Physics simulation Path planning

Context Two possible motion planning abstraction levels: C-space level (more control and generality) Input: C-obstacles and endpoints Output: a path Robot level (more convenience) Input: robot, workspace obstacles, endpoints or high-level tasks Output: a path Comparison to other packages: Open Motion Planning Library (OMPL): implements many PRM-style planning algorithms at C-space level MoveIt! and OpenRAVE: work at robot level (robot obstacle avoidance and

manipulation problems, respectively) and set up C-space constraints for other planners Klampt Implements many PRM-style planners at C-space level (& interface to OMPL) Supports some common problem setups at robot level (e.g., robot avoiding obstacles, with/without IK constraints) Kinematic planning pipeline 1. Construct a planning problem C-space Terminal conditions (start and goal configurations, or in general, sets)

2. Instantiate a planning algorithm Take care: some algorithms work with some problems and not others 3. Call the planner Sampling-based planners are set up for use in any-time fashion Plan as long as you want in a while loop, OR Set up a termination criterion Likelihood of success increases as more time spent For optimizing planners, quality of path improves too 4. Retrieve the path (sequence of milestones)

Setting up a kinematic C-space C-Space-level interface: you must manually implement the callbacks used by the planning algorithm Feasibility tester IsFeasible?(q) Visibility tester IsVisible?(a,b) Sampling strategy q <- SampleConfig() *Perturbation sampling strategy q <- SampleNeighborhood(c,r)

*Distance metric d <- Distance(a,b) *Interpolation function q <- Interpolate(a,b,u) *: default implementation assumes Cartesian space Robot-level interface: you provide a world containing a robot Standard Robot C-Space: avoids collisions Contact C-space: avoids collisions, maintains IK constraints Stance C-space: same as Contact C-space, but also enforces balance under gravity Python API (CSpace) To set up your own problem, construct a subclass of the CSpace class whose methods will be called by the planner class MyCSpace(CSpace):

def __init__(self): CSpace.__init__(self) At minimum, set up the following: bound: a list of pairs [(a1,b1),,(an,bn)] giving an n-dimensional bounding box containing the free space eps: a visibility collision checking tolerance (more later) feasible(q): the feasibility test. Returns True if feasible, False if infeasible Python API (CSpace) more functionality visible(a,b): used for custom visibility tests

sample(): returns a random configuration. Used for custom sampling distributions. Default implementation uses uniform distribution over self.bound. distance(a,b): returns a distance value between configurations a and b. Default uses Euclidean distance interpolate(a,b,u): returns a configuration u fraction of the way toward b from a. Default uses linear interpolation. sampleneighborhood(c,r): used by some planners to sample a random configuration within a neighborhood r of the configuration c. Default samples over box of width 2r intersected with self.bound Static vs. Dynamic Collision Detection

Static checks Dynamic checks Static Feasibility Checking Override it to test whether a configuration is feasible CSpace.IsFeasible(q) (C++ API) CSpace.feasible(q) (Python API) An authoritative representation of C-obstacles Will be called thousands of times during planning. For sampling-based planners to work well, this must be fast (ideally, microseconds) Geometric primitives, or bounding-volume hierarchies

BVHs automatically set up in Klampt on robot or geometry load Dynamic Feasibility Checking Callback that tests whether a simple path between two configurations is feasible (local planning) CSpace.LocalPlanner(a,b)->EdgePlanner* (C++ API) CSpace.visible(a,b) (Python API) C++ API allows non-straight line paths, Python assumes straight-line paths Will be called thousands of times during planning (hopefully far less, in lazy planners). For sampling-based planners to work well, this must be fast (ideally, milliseconds) Default implementations use discretize-and-check approach

Usual Approach to Dynamic Checking (in PRM Planning) 1) Discretize path at some fine resolution e 2) Test statically each intermediate configuration 3 2 3 1 3 2 3

e too large collisions are missed e too small slow test of local paths Testing Path Segment vs. Finding First Collision PRM planning Detect collision as quickly as possible Bisection strategy Physical simulation, haptic interaction Find first collision Sequential strategy e too large collisions are missed

e too small slow test of local paths e too large collisions are missed e too small slow test of local paths Distance metrics Most PRM-style planners rely heavily on the notion of a distance metric PRM (and similar planners): determines set of nearby neighbors for connection. Either k-nearest neighbors, OR All neighbors within radius R RRT (and similar planners): determines how far

to extend tree on each iteration Max distance d This choice is often nontrivial due to axes with different units Example: how to weight the angular component in SE(2)? & Tradeoff between fast exploration vs finding intricate movements in narrow passages Large jumps more effective

Small jumps more effective Interpolation Interpolate(a,b,u) traces out a simple path (e.g., straight line or geodesic) from a->b as u goes from 0->1 C++: CSpace.Interpolate(a,b,u,Config& q) with q used as output Python: q<-CSpace.interpolate(a,b,u) Output of planner: sequence of milestones m0,,mn such subsequent application the C-spaces interpolation function traces out path q(t) = Interpolate(mi,mi+1, t*n-i) with i=floor(tn)

Straight line is most appropriate for Euclidean spaces (default) Geodesic is most appropriate choice for non-Euclidean spaces Klampt planning problems Motion planning problem types Constraints: Kinematic constraints Manifold constraints (must be handled in C-space) Dynamic constraints partially supported Objectives:

Minimum path length well supported Minimum execution time under dynamics partially supported Arbitrary cost functions partially supported Minimum constraint removal Minimum constraint displacement Terminal conditions: point-to-point, point-to-set (not thoroughly tested), multi-query (some planners) Implementation:

C++: fullest support. Not every combination of constraints + objectives + terminal constraints is supported Python: point-to-point kinematic planning, manifold constraints can be handled, only path-length supported as optimization function Klampt planning algorithms Feasible planners: only care about the first feasible solution

Probabilistic Roadmap (PRM) [Kavraki et al 1996] Rapidly-Exploring Random Tree (RRT) [LaValle and Kuffner 2001] Expansive Space Trees (EST ) [Hsu et al 1999] Single-Query Bidirectional Lazy Planner (SBL) [Sanchez-Ante and Latombe 2004] Probabilistic Roadmap of Trees [Akinc et al 2005] w/ SBL (SBL-PRT) Multi-Modal PRM (MMPRM), Incremental-MMPRM [Hauser and Latombe 2009] Optimizing planners: incrementally improve the solution quality over time (path length)

RRT* [Karaman and Frazzoli 2009] PRM* [Karaman and Frazzoli 2009] Lazy-PRM*, Lazy-RRG* [Hauser 2015] Lower-Bound RRT* (LB-RRT*) [Salzman and Halperin 2014] Fast Marching Method (FMM) [Sethian 1996] Asymptotically optimal FMM (FMM*) [Luo and Hauser 2014] Minimum Constraint Removal (MCR) and Minimum Constraint Displacement (MCD) [Hauser 2013] Randomized path shortcutting Random restarts

Implementation: C++: fullest support. Automatic configuration interface for PRM, RRT, EST, SBL, SBL-PRT, RRT*, PRM*, LazyPRM*, Lazy-RRG*, LB-RRT*, FMM, FMM*, shortcutting, restarts Python: Only supports automatic configuration interface Python API (MotionPlan) Sets up a motion planner for a given CSpace First call global configuration MotionPlan.setOptions(option1=value1,,optionk=valuek) Then create the planner, set up the endpoints, and call in a loop

planner = MotionPlan(cspace) planner.setEndpoints(qstart,qgoal) increment = 100 while [TIME LEFT]: planner.planMore(increment) path = planner.getPath if len(path) > 0: print Solved; break Note: if you are creating lots of MotionPlans you will want to call planner.close() to free memory after using it Note: many planners require start and goal to be feasible or throw an exception

Python API (MotionPlan) Planner options MotionPlan.setOptions(option 1=value1,,optionk=valuek) Common options: type: identifies the planning algorithm. Can be prm, rrt, est, sbl, sblprt, rrt*, prm*, lazyprm*, lazyrrg*, fmm, fmm* connectionThreshold: used in prm, rrt perturbationRadius: used in rrt, est, sbl, sblprt bidirectional: if true, performs bidirectional planning. Used in rrt, rrt*, lazyrrg* pointLocation: point location method. [empty]: nave, kdtree: KD-Tree, randombest [k]: best of k random samples shortcut: 0 or 1, indicates whether to perform shortcutting after planning restart: 0 or 1, indicates whether to perform random restarts (used with

restartTermCond) restartTermCond: a JSON string indicating the termination criterion for each random restart. Options can also be loaded from a JSON string (see Examples/PlanDemo/*.settings) via motionplanning.setPlanJSONString(string) Next time Robot physics and dynamic systems Other Approaches to Dynamic Collision Detection Bounding-volume (BV) hierarchies

Discretization issue Feature-tracking methods [Lin, Canny, 91] [Mirtich, 98] V-Clip [Cohen, Lin, Manocha, Ponamgi, 95] I-Collide [Basch, Guibas, Hershberger, 97] KDS Geometric complexity issue with highly non-convex objects Sequential strategy (first collision) that is not efficient for PRM path segments Swept-volume intersection [Cameron, 85] [Foisy, Hayward, 93]

Swept-volumes are expensive to compute. Too much data. No pre-computed BV hierarchies Algebraic trajectory parameterization [Canny, 86] [Schweikard, 91] [Redon, Kheddar, Coquillard, 00] High-degree polynomials, expensive Floating-point arithmetics difficulties Sequential strategy Combination

[Redon, Kheddar, Coquillard, 00] BVH + algebraic parameterization [Ehmann, Lin, 01] BVH + feature tracking Sequential strategy Exact Collision Detection with Adaptive Bisection Idea: Cover line segment with collision free C-space neighborhoods Use distance computation instead of collision checking How do you show a C-space neighborhood is collision free? Relate changes in C-space to changes in workspace

Distance R When moving from (x,y,q) to (x,y,q), no point traces out more than distance |x-x| + |y-y| + R|q-q| How do you show a C-space neighborhood is collision free? Relate changes in C-space to changes in workspace q3 q2 q1 q = (q1,q2,q3)

q = (q1,q2,q3) dqi = qi-qi For any q and q no robot point traces a path longer than: l(q,q) = 3|dq1|+2|dq2|+|dq3| How do you show a C-space neighborhood is collision free? Relate changes in C-space to changes in workspace q3 q2 q1 d(q)

d(q) = Euclidean distance between robot and obstacles (or lower bound) If l(q,q) < d(q) + d(q) then the straight path between q and q is collision-free l(q,q) < d(q) + d(q) q q {q | l(q,q) < d(q)} {q | l(q,q) < d(q)}

l(q,q) = l(q,qint) + l(qint,q) < d(q) + d(q) l(q,q) < d(q) + d(q) q qint q {q | l(q,q) < d(q)} {q | l(q,q) < d(q)} l(q,q) > d(q) + d(q) Bisection q

q {q | l(q,q) < d(q)} {q | l(q,q) < d(q)}