Anna Yershova Thesis Defense Dept. of Computer Science, University of Illinois August 5, 2008 Sampling and Searching Methods for Practical Motion Planning Algorithms Anna Yershova Thesis Defense

 Introduction  Motion Planning  Incremental Sampling and Searching (ISS) Framework  Thesis Overview  Technical Contributions  Nearest Neighbor Searching  Uniform Deterministic Sampling  Guided Sampling  Conclusions and DiscussionIntroduction Presentation Overview Anna Yershova Thesis Defense

Given:,, Initial and goal configurations Extensions: Task:  Compute a collision free path that connects initial and goal configurationsIntroductionMotion Planning The Motion Planning Problem Anna Yershova Thesis Defense [J. Cortes]

Given:,, Initial and goal configurations Extensions: Task:  Compute a collision free path that connects initial and goal configurationsIntroductionMotion Planning The Motion Planning Problem Anna Yershova Thesis Defense

Conceptually simple, but in reality… obstacles in C-spaces are not explicitly defined they are described by an astronomical number of geometric primitives free C-spaces have complicated topologies feasible configurations may lie on lower dimensional algebraic varieties, which are also not explicitly definedIntroductionMotion Planning The Motion Planning Problem Anna Yershova Thesis Defense

 Automotive AssemblyIntroductionMotion PlanningApplicationsApplications Anna Yershova Thesis Defense [Yershova, et. al., 2005] Courtesy of Kineo CAM The solution path traverses a narrow passage in SE(3) [Yershova, et. al., 2005] Courtesy of Kineo CAM The solution path traverses a narrow passage in SE(3)

 Automotive Assembly  Computational Chemistry and BiologyIntroductionMotion PlanningApplicationsApplications Anna Yershova Thesis Defense [Yershova, et. al., 2005] Courtesy of LAAS 330 dimensional C-space [Yershova, et. al., 2005] Courtesy of LAAS 330 dimensional C-space

 Automotive Assembly  Computational Chemistry and Biology  Manipulation Planning  Medical applications  Computer Graphics (motions for digital actors)  Autonomous vehicles and spacecraftsIntroductionMotion PlanningApplicationsApplications Anna Yershova Thesis Defense courtesy of Volvo Cars and FCC

 Grid Sampling, AI Search (beginning of time-1977)  Experimental mobile robotics, etc.  Problem Formalization ( )  Configuration space (Lozano-Perez, )  PSPACE-hardness (Reif, 1979)  Combinatorial Solutions ( )  Cylindrical algebraic decomposition (Schwartz, Sharir, 1983)  Stratifications, roadmap (Canny, 1987)  Sampling-based Planning (1988-present)  Randomized potential fields (Barraquand, Latombe, 1989)  Ariadne's clew algorithm (Ahuactzin, Mazer, 1992)  Probabilistic Roadmaps (PRMs) (Kavraki, Svestka, Latombe, Overmars, 1994)  Rapidly-exploring Random Trees (RRTs) (LaValle, Kuffner, 1998)IntroductionMotion PlanningHistoryHistory Anna Yershova Thesis Defense “black box” Collision detection is used as a “black box”

x goal x init Build a graph over the configuration space that connects initial and goal configurations: 1.Graph is embedded in C-space 2.Every vertex is a configuration 3.Every edge is a pathIntroductionISS Framework Incremental Sampling and Searching Framework Anna Yershova Thesis Defense

IntroductionISS Framework Typical Architecture Anna Yershova Thesis Defense Uniform Sampling Guided Sampling Nearest Neighbor Search Collision Detection Path Exists ? no yes Solution path Input geometry

Thesis OverviewIntroduction Central Theme Anna Yershova Thesis Defense The performance of motion planning algorithms can be significantly improved by identifying and addressing the key issues in sampling and searching framework. ISSUES ADDRESSED: efficient nearest-neighbor computations uniform deterministic sampling over configuration spaces guided sampling for efficient exploration Thesis Overview: Chapter 1: Introduction Chapter 2: ISS Framework Chapter 3: Nearest Neighbor Search Chapter 4: Uniform Sampling Chapter 5: Guided Sampling

 Introduction  Motion Planning  ISS Framework  Thesis Overview  Technical Contributions  Nearest Neighbor Searching  Uniform Deterministic Sampling  Guided Sampling  Conclusions and Discussion Presentation Overview Anna Yershova Thesis Defense Technical Approach Nearest Neighbor Search

MotivationMotivation Anna Yershova Thesis Defense ISS methods often compute the nearest vertex in the graph Technical Approach Nearest Neighbor Search q

Problem Formulation Anna Yershova Thesis Defense Given: a d -dimensional manifold, T, and a set of data points in T Goal: preprocess these points so that, for any query point q in T, the nearest data point to q can be found quickly Manifolds of interest: Technical Approach Nearest Neighbor Search Euclidean space, [0,1] d Spheres, S d Projective space, R P 3 Cartesian products of the above

Problem Formulation Anna Yershova Thesis Defense Given: a d -dimensional manifold, T, and a set of data points in T Goal: preprocess these points so that, for any query point q in T, the nearest data point to q can be found quickly Manifolds of interest: Technical Approach Nearest Neighbor Search Euclidean space, [0,1] d Hyperspheres, S d Projective space, R P 3 Cartesian products of the above Hypercube embedded in R d with Euclidean metric

Problem Formulation Anna Yershova Thesis Defense Given: a d -dimensional manifold, T, and a set of data points in T Goal: preprocess these points so that, for any query point q in T, the nearest data point to q can be found quickly Manifolds of interest: Technical Approach Nearest Neighbor Search Euclidean space, [0,1] d Hyperspheres, S d Projective space, R P 3 Cartesian products of the above d-sphere embedded in R d+1 with induced metric

Problem Formulation Anna Yershova Thesis Defense Given: a d -dimensional manifold, T, and a set of data points in T Goal: preprocess these points so that, for any query point q in T, the nearest data point to q can be found quickly Manifolds of interest: Technical Approach Nearest Neighbor Search Euclidean space, [0,1] d Hyperspheres, S d Projective space, R P 3 Cartesian products of the above, metric compatible with Haar measure

Problem Formulation Anna Yershova Thesis Defense Given: a d -dimensional manifold, T, and a set of data points in T Goal: preprocess these points so that, for any query point q in T, the nearest data point to q can be found quickly Manifolds of interest: Technical Approach Nearest Neighbor Search Euclidean space, [0,1] d Hyperspheres, S d Projective space, R P 3 Cartesian products of the above weighed metric

Technical Approach Nearest Neighbor Search Example: Torus, S 1 xS 1 Anna Yershova Thesis Defense q Universal cover of torus allows visualization of the nearest neighbor search

Literature review Anna Yershova Thesis Defense Euclidean spaces: [Friedman, 77] [Sproull, 91] [Arya, 93] [Agarwal, 02] [indyk, 04] The most successful method used in practice is based on kd-trees [Arya 93] General metric spaces: Consider metric as a “black box” [Clarkson, 03,05] [Beygelzimer, 04] [Krauthgamer, 04] [Hjaltason, 03] The spaces we consider are manifolds, i.e. locally Euclidean, with identifications on the boundary. This allows extension of kd-trees. Technical Approach Nearest Neighbor Search

The kd-tree is a data structure based on recursively subdividing a set of points with alternating axis-aligned hyperplanes l5l5 l1l1 l9l9 l6l6 l3l3 l 10 l7l7 l4l4 l8l8 l2l2 l1l1 l8l8 1 l2l2 l3l3 l4l4 l5l5 l7l7 l6l6 l9l Technical Approach Nearest Neighbor Search Kd-trees for [0,1] d Anna Yershova Thesis Defense

q l5l5 l1l1 l9l9 l6l6 l3l3 l 10 l7l7 l4l4 l8l8 l2l2 l1l1 l8l8 1 l2l2 l3l3 l4l4 l5l5 l7l7 l6l6 l9l Technical Approach Nearest Neighbor Search Query phase for [0,1] 2 Anna Yershova Thesis Defense

l5l5 l1l1 l9l9 l6l6 l3l3 l 10 l7l7 l4l4 l8l8 l2l2 Technical Approach Nearest Neighbor Search Kd-trees with modified metric Anna Yershova Thesis Defense Main idea: construction: unchanged procedure query: modify metric between the query point and enclosing rectangles in the kd-tree Main idea: construction: unchanged procedure query: modify metric between the query point and enclosing rectangles in the kd-tree l1l1 l8l8 1 l2l2 l3l3 l4l4 l5l5 l7l7 l6l6 l9l9 l [0,1]xS 1

q l5l5 l1l1 l9l9 l6l6 l3l3 l 10 l7l7 l4l4 l8l8 l2l2 1 3 l4l4 l8l8 l2l2 l1l1 l8l8 1 l2l2 l3l3 l4l4 l5l5 l7l7 l6l6 l9l Technical Approach Nearest Neighbor Search Query phase with modified metric Anna Yershova Thesis Defense [0,1]xS 1

Technical Approach Nearest Neighbor Search Analysis of the algorithm Anna Yershova Thesis Defense Proposition 1. The algorithm correctly returns the nearest neighbor. Proof idea: The points of kd-tree not visited by an algorithm will always be farther from the query point than some point already visited. Proposition 2. For n points in dimension d, the construction time is O(dn lgn), the space is O(dn), and the query time is logarithmic in n, but exponential in d. Proof idea: This follows directly from the well-known complexity of the basic kd-tree.

For 50,000 data points, 100 queries were made: Technical Approach Nearest Neighbor SearchExperimentsExperiments Anna Yershova Thesis Defense

Technical Approach Nearest Neighbor Search Anna Yershova Thesis Defense ExperimentsExperiments

Technical Approach Nearest Neighbor Search Anna Yershova Thesis Defense Publications: Improving Motion Planning Algorithms by Efficient Nearest Neighbor Searching Anna Yershova and Steven M. LaValle IEEE Transactions on Robotics 23(1): , February 2007 Publicly available library: Also implemented in Move3D at LAAS, and KineoWorks TM OutcomesOutcomes

 Introduction  Motion Planning  ISS Framework  Thesis Overview  Technical Contributions  Nearest Neighbor Searching  Uniform Deterministic Sampling  Uniform Deterministic Sampling (partly in collaboration with Julie C. Mitchell)  Guided Sampling  Conclusions and Discussion Presentation Overview Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling

Technical Approach Uniform Deterministic SamplingMotivationMotivation Anna Yershova Thesis Defense The graph over C-space should capture the “path connectivity” of the space

Problem Formulation Anna Yershova Thesis Defense Desirable properties of samples over the C-space: Technical Approach Uniform Deterministic Sampling uniform deterministic incremental grid structure uniform deterministic incremental grid structure

Desirable properties of samples over the C-space: Problem Formulation Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling uniform deterministic incremental grid structure uniform deterministic incremental grid structure Discrepancy: maximum volume estimation error Dispersion: the radius of the largest empty balls

Problem Formulation Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling uniform deterministic incremental grid structure uniform deterministic incremental grid structure The uniformity measures can be deterministically computed Reason: resolution completeness The uniformity measures can be deterministically computed Reason: resolution completeness Desirable properties of samples over the C-space:

Problem Formulation Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling uniform deterministic incremental grid structure uniform deterministic incremental grid structure The uniformity measures are optimized with every new point Reason: it is unknown how many points are needed to solve the problem in advance The uniformity measures are optimized with every new point Reason: it is unknown how many points are needed to solve the problem in advance Desirable properties of samples over the C-space:

Problem Formulation Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling uniform deterministic incremental grid structure uniform deterministic incremental grid structure Reason: Trivializes nearest neighbor computations Desirable properties of samples over the C-space:

Desirable properties of samples over the C-space: Problem Formulation Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling uniform deterministic incremental grid structure uniform deterministic incremental grid structure Euclidean space, [0,1] d Spheres, S d Projective space, R P 3 Cartesian products of the above

Literature overview Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling Euclidean space, [0,1] d Spheres, S d Special orthogonal group, SO(3)

Literature Overview: Euclidean Spaces, [0,1] d Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling + uniform + deterministic + incremental  grid structure + uniform + deterministic + incremental  grid structure + uniform + deterministic + incremental  grid structure + uniform + deterministic + incremental  grid structure + uniform  deterministic + incremental  grid structure + uniform  deterministic + incremental  grid structure + uniform + deterministic  incremental  grid structure + uniform + deterministic  incremental  grid structure + uniform + deterministic  incremental  grid structure + uniform + deterministic  incremental  grid structure Halton points Hammersley points Random sequence Sukharev gridA lattice

Literature Overview: Euclidean Spaces, [0,1] d Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling Layered Sukharev Grid Sequence [Lindemann, LaValle 2003] + uniform + deterministic + incremental  grid structure + uniform + deterministic + incremental  grid structure

Literature Overview: Spheres, S d, and SO(3) Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling  Random sequences  subgroup method for random sequences SO(3)  almost optimal discrepancy random sequences for spheres [Beck, 84] [Diaconis, Shahshahani 87] [Wagner, 93] [Bourgain, Linderstrauss 93]  Deterministic point sets  optimal discrepancy point sets for SO(3)  uniform deterministic point sets for SO(3) [Lubotzky, Phillips, Sarnak 86] [Mitchell 07]  No deterministic sequences to our knowledge + uniform  deterministic + incremental  grid structure + uniform  deterministic + incremental  grid structure + uniform  deterministic  incremental  grid structure + uniform  deterministic  incremental  grid structure

Our approach: Spheres Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling +/  uniform  deterministic + incremental  grid structure +/  uniform  deterministic + incremental  grid structure Ordering on faces + Ordering inside faces Make a Layered Sukharev Grid sequence inside each face Define the ordering across faces Combine these two into a sequence on the cube Project the faces of the cube outwards to form spherical tiling Use barycentric coordinates to define the sequence on the sphere

Our approach: Cartesian Products Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling X  YX  Y Make grid cells inside X and Y Naturally extend the grid structure to X  Y Define the cell ordering and the ordering inside each cell X Y X  YX  Y Ordering on cells, Ordering inside cells

Our approach: SO(3) Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling Hopf coordinates preserve the fiber bundle structure of RP 3 Locally, RP 3 is a product of S 1 and S 2 Joint work with J.C.Mitchell

Our approach:SO(3) Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling The method for Cartesian products can then be applied to R P 3 Need two grids, for S 1 and S 2 Healpix, [Gorski,05] Grid on S 2 Grid on S 1

Our approach:SO(3) Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling The method for Cartesian products can then be applied to R P 3 Need two grids, for S 1 and S 2 Grid on S 2 Grid on S 1

Our approach:SO(3) Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling The method for Cartesian products can then be applied to R P 3 Need two grids, for S 1 and S 2 Ordering on faces, ordering on [0,1] 3 Grid on S 2 Grid on S 1 + uniform  deterministic + incremental  grid structure + uniform  deterministic + incremental  grid structure

1. The dispersion of the sequence T s at the resolution level l containing points is: 2. The relationship between the discrepancy of the sequence T at the resolution level l taken over d -dimensional spherical canonical rectangles and the discrepancy of the optimal sequence, T o, is: 3. The sequence T has the following properties: The position of the i -th sample in the sequence T can be generated in O ( log i ) time.  For any i -th sample any of the 2 d nearest grid neighbors from the same layer can be found in O (( log i )/ d ) time. PropositionsPropositions Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling

1. The dispersion of the sequence T at the resolution level l is: in which is the dispersion of the sequence over S 2. PropositionsPropositions Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling

ExperimentsExperiments Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling Configuration spaces: SO(3) and SE(3) = R 3 x SO(3) (a) (b)

ExperimentsExperiments Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling Configuration spaces: SO(3) and SE(3) = R 3 x SO(3) (c)(d)

OutcomesOutcomes Anna Yershova Thesis Defense Technical Approach Uniform Deterministic Sampling Publications: Generating Uniform Incremental Grids on SO(3) Using the Hopf Fibration Anna Yershova, Steven M. LaValle, and Julie C. Mitchell Submitted to the Eighth International Workshop on the Algorithmic Foundations of Robotics (WAFR 2008) Deterministic sampling methods for spheres and SO(3) Anna Yershova and Steven M. LaValle, 2004 IEEE International Conference on Robotics and Automation (ICRA 2004) Incremental Grid Sampling Strategies in Robotics Stephen R. Lindemann, Anna Yershova, and Steven M. LaValle, Sixth International Workshop on the Algorithmic Foundations of Robotics (WAFR 2004) Publicly available library:

 Introduction  Motion Planning  ISS Framework  Thesis Overview  Technical Contributions  Nearest Neighbor Searching  Uniform Deterministic Sampling  Guided Sampling  Guided Sampling (partly in collaboration with N. Simeon, L. Jaillet)  Conclusions and Discussion Presentation Overview Anna Yershova Thesis Defense Technical Approach Guided Sampling

Rapidly-Exploring Random Trees Anna Yershova Thesis Defense Technical Approach Guided Sampling [LaValle, Kuffner 99]

Voronoi-Biased Exploration Anna Yershova Thesis Defense Technical Approach Guided Sampling Is this always a good idea?

Voronoi Diagram Anna Yershova Thesis Defense Technical Approach Guided Sampling

refinementexpansion Where will the random sample fall? How to control the behavior of RRT? Refinement vs. Expansion Anna Yershova Thesis Defense Technical Approach Guided Sampling

Expansion dominates Balanced refinement and expansion The trade-off depends on the size of the bounding box Determining the Boundary Anna Yershova Thesis Defense Technical Approach Guided Sampling

Refinement is good when multiresolution search is needed Expansion is good when the tree can grow and will not be blocked by obstacles Main motivation: Voronoi bias does not take into account obstacles How to incorporate the obstacles into Voronoi bias? Controlling the Voronoi Bias Anna Yershova Thesis Defense Technical Approach Guided Sampling

Which one will perform better? Small Bounding Box Large Bounding Box Technical Approach Guided Sampling Bug Trap Anna Yershova Thesis Defense

Technical Approach Guided Sampling Voronoi Bias for the Original RRT Anna Yershova Thesis Defense Instead of fixed sampling domain use Dynamic Domain for sampling

Technical Approach Guided Sampling Collision Detection-Based Dynamic Domain Anna Yershova Thesis Defense Rejection-based implementation. Only works in up to 4 dimensions.

Technical Approach Guided Sampling KD-Tree-Based Dynamic Domain Anna Yershova Thesis Defense

Technical Approach Guided Sampling KD-Tree-Based Dynamic Domain Anna Yershova Thesis Defense

Technical Approach Guided Sampling KD-Tree-Based Dynamic Domain Anna Yershova Thesis Defense A computed example of the kd-tree sampling domain

Wiper Motor (courtesy of KINEO) 6 dof problem CD calls are expensive Technical Approach Guided Sampling Experiments: Industrial Benchmark Anna Yershova Thesis Defense

Molecule 68 dof problem was solved in 2 minutes, never solved before 330 dof in 1 hour, never solved before 6 dof in 1 min, has 30 times improvement compared to RRT CD calls are expensive Technical Approach Guided Sampling Experiments: Protein Docking Anna Yershova Thesis Defense

Experiments: Closed Chains Technical Approach Guided Sampling Anna Yershova Thesis Defense

Uniform sampling on hyperspheres Sampling on C-spaces arising from closed chains Sampling in the space of trajectories for control systems: motion primitivesConclusions Anna Yershova Thesis Defense ConclusionsConclusions Thank you!