1. Tree growing motion planners + optimizing planners

2. Probabilistic Roadmaps
What if only a small portion of the space needs to be explored?

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

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

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

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

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

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

9. 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
xrand
xnear

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

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

12. What is the sampling strategy?
Probability that a node gets selected for expansion is proportional to the volume of its Voronoi cell

13. Implementation Details
Bottleneck: finding nearest neighbor each step
O(n) with naïve 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

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

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

16. RRT
RRT*

17. Is this the most efficient method?
No! an excessive number of samples is typically needed to improve an existing path

18. Some Theory
Assume there is an 𝜖-optimal shortest path with clearance 𝛿/2 from obstacles Binary collision checks are the only information available to planner about shape of obstacles

19. Some Theory
Assume there is an 𝜖-optimal shortest path with clearance 𝛿/2 from obstacles Binary collision checks are the only information available to planner about shape of obstacles
Filled?
Or free?

20. Dimensionality matters!
Conclude
For this 2D example, there exists a set of volume 𝛿 2 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( 𝜖 −d )
Worst case: no better than grid search!
Dimensionality matters!
Visibility doesn’t
Sampling strategy: check configurations where they help find a better path

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

22. Motion Planning in Klamp’t
Kris Hauser
ECE 383 / ME 442 22

23. Agenda Motion planning in Klamp’t Key concepts Klamp’t Python API
CSpace
Feasibility checker (static collision checking)
Visibility checker (dynamic collision checking)
MotionPlan
Klamp’t Python API

24. 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
Robot posing
Motion design
ROS interface
JSON / Sockets
Physics simulation
Path planning
Disk I/O

25. 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
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
Klamp’t
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)

26. Kinematic planning pipeline
Construct a planning problem
C-space
Terminal conditions (start and goal configurations, or in general, sets)
Instantiate a planning algorithm
Take care: some algorithms work with some problems and not others
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
Retrieve the path (sequence of milestones)

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

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

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

30. Static vs. Dynamic Collision Detection
Static checks
Dynamic checks

31. 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 Klamp’t on robot or geometry load

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

33. Usual Approach to Dynamic Checking (in PRM Planning)
Discretize path at some fine resolution e
Test statically each intermediate configuration
< e 3 2 3 1 3 2 3
e too large  collisions are missed
e too small  slow test of local paths

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

35. e too large  collisions are missed
e too small  slow test of local paths

36. e too large  collisions are missed
e too small  slow test of local paths

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

38. 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-space’s 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

39. Klamp’t 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

40. Klamp’t 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*, Lazy-PRM*, Lazy-RRG*, LB-RRT*, FMM, FMM*, shortcutting, restarts
Python: Only supports automatic configuration interface

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

42. Python API (MotionPlan) Planner options
MotionPlan.setOptions(option1=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]: naïve, “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)

43. Next time
Robot physics and dynamic systems

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

45. Exact Collision Detection with Adaptive Bisection
Idea: Cover line segment with collision free C-space neighborhoods
Use distance computation instead of collision checking

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

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

48. d(q) = Euclidean distance between robot and obstacles (or lower bound)
How do you show a C-space neighborhood is collision free?
Relate changes in C-space to changes in workspace
d(q) = Euclidean distance between robot and obstacles
(or lower bound) q1 q2 q3
d(q)
If l(q,q’) < d(q) + d(q’)
then the straight path between q and q’ is collision-free

49. l(q,q’) < d(q) + d(q’)
{q” | l(q,q”) < d(q)}
{q” | l(q’,q”) < d(q’)}

50. l(q,q’) = l(q,qint) + l(qint,q’) < d(q) + d(q’)
l(q,q’) < d(q) + d(q’) q q’
{q” | l(q,q”) < d(q)}
{q” | l(q’,q”) < d(q’)}
qint

51. l(q,q’) > d(q) + d(q’)
Bisection q’ q
{q” | l(q’,q”) < d(q’)}
{q” | l(q,q”) < d(q)}