anna yershova thesis defense dept. of computer science, university of illinois august 5, 2008

Anna Yershova

Thesis Defense

Dept. of Computer Science, University of Illinois

August 5, 2008

Sampling and Searching Methodsfor Practical Motion Planning Algorithms

Anna YershovaAnna Yershova Thesis Defense

X

ksfhjdsf

IntroductionIntroduction Motion Planning Incremental Sampling and Searching (ISS) Framework Thesis Overview

Technical Contributions Nearest Neighbor Searching Uniform Deterministic Sampling Guided Sampling

Conclusions and Discussion

IntroductionIntroduction

Presentation OverviewPresentation OverviewPresentation OverviewPresentation Overview


Given:

• , ,

• Initial and goal configurations

Extensions:

Task: Compute a collision free path that connects initial and goal

configurations

IntroductionIntroduction Motion Planning

The Motion Planning ProblemThe Motion Planning ProblemThe Motion Planning ProblemThe Motion Planning Problem


[J. Cortes]

Given:

• , ,

• Initial and goal configurations

Extensions:

Task: Compute a collision free path that connects initial and goal

configurations




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 defined




Automotive Assembly


ApplicationsApplicationsApplicationsApplications


[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 Chemistryand Biology




[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 Chemistryand Biology

Manipulation Planning

Medical applications

Computer Graphics(motions for digital actors)

Autonomous vehicles andspacecrafts




courtesy of Volvo Cars and FCCcourtesy of Volvo Cars and FCC

Grid Sampling, AI Search (beginning of time-1977) Experimental mobile robotics, etc.

Problem Formalization (1977-1983) Configuration space (Lozano-Perez, 1978-1981) PSPACE-hardness (Reif, 1979)

Combinatorial Solutions (1983-1988) 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)


HistoryHistoryHistoryHistory


Collision detection is used as a “black box”“black box”

Collision detection is used as a “black box”“black box”

xgoal

xinit

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 path

IntroductionIntroduction ISS Framework

Incremental Sampling and Searching Framework Incremental Sampling and Searching Framework Incremental Sampling and Searching Framework Incremental Sampling and Searching Framework


IntroductionIntroduction ISS Framework

Typical ArchitectureTypical ArchitectureTypical ArchitectureTypical Architecture


Uniform SamplingUniform Sampling

Guided SamplingGuided Sampling

Nearest Neighbor SearchNearest Neighbor Search

Collision DetectionCollision Detection

Path Exists ?Path Exists ?no yes

Solution

path

Input

geometry

Thesis OverviewIntroductionIntroduction

Central ThemeCentral ThemeCentral ThemeCentral Theme


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: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 SearchingNearest Neighbor Searching Uniform Deterministic Sampling Guided Sampling




Technical ApproachTechnical Approach Nearest Neighbor Search

MotivationMotivationMotivationMotivation


ISS methods often compute the nearest vertex

in the graph


q

Problem FormulationProblem FormulationProblem FormulationProblem Formulation


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:


• Euclidean space, [0,1]d

• Spheres, Sd

• Projective space, R P3

• Cartesian products of the above









• Hyperspheres, Sd



Hypercube embedded in R d with Euclidean metricHypercube embedded in R d with Euclidean metric












d-sphere embedded in R d+1 with induced metricd-sphere embedded in R d+1 with induced metric












, metric compatible with Haar measure

, metric compatible with Haar measure











• Cartesian products of the aboveweighed metricweighed metric


Example: Torus, Example: Torus, SS11xxSS11Example: Torus, Example: Torus, SS11xxSS11


47

6

5

1

3

2

9

8

10

11

q

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

Universal cover of torus allows visualization of the nearest neighbor

search

Universal cover of torus allows visualization of the nearest neighbor

search

Literature reviewLiterature reviewLiterature reviewLiterature review


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.


The kd-tree is a data structure based on recursively subdividing a set of points with alternating axis-aligned hyperplanes.

47

6

5

1

3

2

9

8

10

11

l5

l1 l9

l6

l3

l10 l7

l4

l8

l2

l1

l8

1

l2l3

l4 l5 l7 l6

l9l10

3

2 5 4 11

9 10

8

6 7


Kd-trees for [0,1]Kd-trees for [0,1]dd Kd-trees for [0,1]Kd-trees for [0,1]dd


q

47

6

5

1

3

2

9

8

10

11

l5

l1 l9

l6

l3

l10 l7

l4

l8

l2

l1

l8

1

l2l3

l4 l5 l7 l6

l9l10

3

2 5 4 11

9 10

8

6 7


Query phase for [0,1]Query phase for [0,1]22 Query phase for [0,1]Query phase for [0,1]22


47

6

5

1

3

2

9

8

10

11

l5

l1 l9

l6

l3

l10 l7

l4

l8

l2


Kd-trees with modified metricKd-trees with modified metricKd-trees with modified metricKd-trees with modified metric


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

l1

l8

1

l2l3

l4 l5 l7 l6

l9l10

3

2 5 4 11

9 10

8

6 7

[0,1]xS1[0,1]xS1

q

47

6

5

1

3

2

9

8

10

11

l5

l1 l9

l6

l3

l10 l7

l4

l8

l2

1

3

l4

l8

l2

l1

l8

1

l2l3

l4 l5 l7 l6

l9l10

3

2 5 4 11

9 10

8

6 7


Query phase with modified metricQuery phase with modified metricQuery phase with modified metricQuery phase with modified metric


[0,1]xS1[0,1]xS1


Analysis of the algorithmAnalysis of the algorithmAnalysis of the algorithmAnalysis of the algorithm


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:


ExperimentsExperimentsExperimentsExperiments




Publications: Improving Motion Planning Algorithms by Efficient Nearest Neighbor

Searching Anna Yershova and Steven M. LaValleIEEE Transactions on Robotics 23(1):151-157, February 2007

Publicly available library: http://msl.cs.uiuc.edu/~yershova/mpnn/mpnn.tar.gz

Also implemented in Move3D at LAAS, and KineoWorksTM

OutcomesOutcomesOutcomesOutcomes


Technical Contributions Nearest Neighbor Searching Uniform Deterministic Sampling Uniform Deterministic Sampling (partly in collaboration with Julie C.

Mitchell) Guided Sampling




Technical ApproachTechnical Approach Uniform Deterministic Sampling


MotivationMotivationMotivationMotivation


The graph over C-space should capture

the “path connectivity” of the space



Desirable properties of samples

over the C-space:


• uniform

• deterministic

• incremental

• grid structure

• uniform

• deterministic

• incremental

• grid structure


over the C-space:




• uniform

• deterministic

• incremental

• grid structure

• uniform

• deterministic

• incremental

• grid structure

Discrepancy:maximum volume estimation error

Dispersion: the radius of the largest empty balls




• 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


over the C-space:




• 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


over the C-space:




• uniform

• deterministic

• incremental

• grid structure

• uniform

• deterministic

• incremental

• grid structureReason: Trivializes nearest neighbor computations

Reason: Trivializes nearest neighbor computations


over the C-space:


over the C-space:




• uniform

• deterministic

• incremental

• grid structure

• uniform

• deterministic

• incremental

• grid structure


• Spheres, Sd



Literature overviewLiterature overviewLiterature overviewLiterature overview




• Spheres, Sd

• Special orthogonal group, SO(3)

Literature Overview: Euclidean Spaces, Literature Overview: Euclidean Spaces, [0,1][0,1]dd




+ 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 pointsHammersley

pointsRandom sequence

Sukharev grid A lattice





Layered Sukharev Grid Sequence[Lindemann, LaValle 2003]

+ uniform

+ deterministic

+ incremental

grid structure

+ uniform

+ deterministic

+ incremental

grid structure

Literature Overview: Spheres, Literature Overview: Spheres, SSdd, and SO(3), and SO(3)Literature Overview: Spheres, Literature Overview: Spheres, SSdd, and SO(3), and SO(3)



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: SpheresOur approach: SpheresOur approach: SpheresOur approach: Spheres



+/ 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 ProductsOur approach: Cartesian ProductsOur approach: Cartesian ProductsOur approach: Cartesian Products



X 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

XY

X Y

Ordering on cells,Ordering inside cells

1234

Our approach: Our approach: SO(3)SO(3)Our approach: Our approach: SO(3)SO(3)



Hopf coordinates preserve the fiber bundle structure of RP3

Locally, RP3 is a product of S1 and S2

Joint work with J.C.Mitchell

Our approach:Our approach:SO(3)SO(3)Our approach:Our approach:SO(3)SO(3)



The method for Cartesian products can then be applied to R P3

Need two grids, for S1 and S2

Healpix, [Gorski,05]Healpix, [Gorski,05]

Grid on S2

Grid on S2

Grid on S1

Grid on S1






Grid on S2

Grid on S2

Grid on S1

Grid on S1






Ordering on faces, ordering on [0,1]3

Grid on S2

Grid on S2

Grid on S1

Grid on S1

+ uniform

deterministic

+ incremental

grid structure

+ uniform

deterministic

+ incremental

grid structure

1. The dispersion of the sequence Ts 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, To, 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 2d nearest grid neighbors from the same

layer can be found in O((log i)/d) time.

PropositionsPropositionsPropositionsPropositions



1. The dispersion of the sequence T at the resolution level l is:

in which is the dispersion of the sequence over S2.

PropositionsPropositionsPropositionsPropositions






Configuration spaces: SO(3) and SE(3) = R3 x SO(3)

(a)

(b)




Configuration spaces: SO(3) and SE(3) = R3 x SO(3)

(c) (d)

OutcomesOutcomesOutcomesOutcomes



Publications: Generating Uniform Incremental Grids on SO(3) Using the Hopf Fibration

Anna Yershova, Steven M. LaValle, and Julie C. MitchellSubmitted 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: http://msl.cs.uiuc.edu/~yershova/sampling/sampling.tar.gz


Technical Contributions Nearest Neighbor Searching Uniform Deterministic Sampling Guided Sampling Guided Sampling (partly in collaboration with N. Simeon, L. Jaillet)




Technical ApproachTechnical Approach Guided Sampling

Rapidly-Exploring Random TreesRapidly-Exploring Random TreesRapidly-Exploring Random TreesRapidly-Exploring Random Trees



[LaValle, Kuffner 99]

Voronoi-Biased ExplorationVoronoi-Biased ExplorationVoronoi-Biased ExplorationVoronoi-Biased Exploration



Is this always a good idea?

Is this always a good idea?

Voronoi DiagramVoronoi DiagramVoronoi DiagramVoronoi Diagram



refinement expansion

Where will the random sample fall? How to control the behavior of RRT?

Refinement vs. ExpansionRefinement vs. ExpansionRefinement vs. ExpansionRefinement vs. Expansion



Expansion dominates

Balanced refinement and expansion

The trade-off depends on the size of the bounding box

Determining the BoundaryDetermining the BoundaryDetermining the BoundaryDetermining the Boundary



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 BiasControlling the Voronoi BiasControlling the Voronoi BiasControlling the Voronoi Bias



Which one will perform better?

Small Bounding Box Large Bounding Box


Bug TrapBug TrapBug TrapBug Trap



Voronoi Bias for the Original RRTVoronoi Bias for the Original RRTVoronoi Bias for the Original RRTVoronoi Bias for the Original RRT


Instead of fixed sampling domain use Dynamic Domain for sampling


Collision Detection-Based Dynamic DomainCollision Detection-Based Dynamic DomainCollision Detection-Based Dynamic DomainCollision Detection-Based Dynamic Domain


Rejection-based implementation. Only works in up to 4 dimensions.


KD-Tree-Based Dynamic DomainKD-Tree-Based Dynamic DomainKD-Tree-Based Dynamic DomainKD-Tree-Based Dynamic Domain



KD-Tree-Based Dynamic DomainKD-Tree-Based Dynamic DomainKD-Tree-Based Dynamic DomainKD-Tree-Based Dynamic Domain


A computed example of the kd-tree sampling domain

Wiper Motor (courtesy of KINEO)

• 6 dof problem• CD calls are

expensive


Experiments: Industrial BenchmarkExperiments: Industrial BenchmarkExperiments: Industrial BenchmarkExperiments: Industrial Benchmark


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


Experiments: Protein DockingExperiments: Protein DockingExperiments: Protein DockingExperiments: Protein Docking


Experiments: Closed ChainsExperiments: Closed ChainsExperiments: Closed ChainsExperiments: Closed ChainsTechnical ApproachTechnical Approach Guided Sampling


Uniform sampling on hyperspheres

Sampling on C-spaces arising from closed chains

Sampling in the space of trajectories for control systems: motion primitives

ConclusionsConclusions


ConclusionsConclusionsConclusionsConclusions

Thank you!

Thank you!

anna yershova thesis defense dept. of computer science, university of illinois august 5, 2008

Documents

samplingbased planning

goal configurationsextensions

fccgrid sampling

collision free path

cspaceevery vertex

narrow passage

ai search beginning

configurationevery edge