On Delaying Collision Checking in PRM Planning – Application to Multi-Robot Coordination

On Delaying Collision On Delaying Collision Checking in PRM Planning – Checking in PRM Planning – Application to Multi-Robot Application to Multi-Robot CoordinationCoordinationBy: Gildardo Sanchez and Jean-Claude Latombe

Presented by:Michael Graeb and Samir Menon

Delayed Collision CheckingDelayed Collision CheckingMotivations

• Experimental Foundations: Collision checks removed from planner

Improved efficiency by 2-3 orders of magnitude Most paths remained collision free

• Most time is spent checking connections• Short connections likely to be collision-free anyway • Most collision-free connections not part of final path

Hence: Postpone testing a connection until it is absolutely needed

Single QuerySingle QueryMotivations

◦ 90% - 99.9% of milestones in multi-query roadmap unused by final path.

◦ Most roadmaps with “good coverage” only used for a single task

Hence: Single Query◦ Build a roadmap with your specific tasks in

mind◦ Bi-Directional trees are an efficient query

technique

S

G

SBL Algorithm – OverallSBL Algorithm – Overall1. Start roadmap with two trees

Rooted at start and goal, one node each

2. Try s times…

SBL Algorithm – OverallSBL Algorithm – Overall2a) Grow a tree by one node

SBL Algorithm – OverallSBL Algorithm – Overall2b) Find a path from start to goal

◦ We’re not certain all edges are valid

Start

Goal

SBL Algorithm – OverallSBL Algorithm – Overall2c) Test unknown edges in path

◦ Stop once a collision is found◦ Remember edges’ validity, for future use

SBL Algorithm – OverallSBL Algorithm – Overall2c) Test unknown edges in path

◦ Stop once a collision is found◦ Remember edges’ validity, for future use

SBL Algorithm – OverallSBL Algorithm – Overall2d) If all edges valid, return path

SBL Algorithm - EXPANDSBL Algorithm - EXPAND1. Pick which tree, T, will receive new

milestone Uniformly random choice, 50/50 odds

SBL Algorithm - EXPANDSBL Algorithm - EXPAND

2. From T, pick an existing milestone, m Random choice, with milestones in less-dense

regions more likely to be picked

SBL Algorithm - EXPANDSBL Algorithm - EXPAND3. Repeat until new milestone created:

3a) Take sample in neighborhood of m Distance from m is initially p and shrinks

with each successive attempt

3b) If sample collision free, add it as child of m

SBL Algorithm - EXPANDSBL Algorithm - EXPAND3. Repeat until new milestone created:

3a) Take sample in neighborhood of m Distance from m is initially p and shrinks

with each successive attempt

3b) If sample collision free, add it as child of m

SBL Algorithm - EXPANDSBL Algorithm - EXPAND3. Repeat until new milestone created:

3a) Take sample in neighborhood of m Distance from m is initially p and shrinks

with each successive attempt

3b) If sample collision free, add it as child of m

SBL Algorithm - EXPANDSBL Algorithm - EXPAND3. Repeat until new milestone created:

3a) Take sample in neighborhood of m Distance from m is initially p and shrinks

with each successive attempt

3b) If sample collision free, add it as child of m

SBL Algorithm - EXPANDSBL Algorithm - EXPAND3. Repeat until new milestone created:

3a) Take sample in neighborhood of m Distance from m is initially p and shrinks

with each successive attempt

3b) If sample collision free, add it as child of m

SBL Algorithm - EXPANDSBL Algorithm - EXPAND3. Repeat until new milestone created:

3a) Take sample in neighborhood of m Distance from m is initially p and shrinks

with each successive attempt

3b) If sample collision free, add it as child of m

SBL Algorithm - EXPANDSBL Algorithm - EXPAND3. Repeat until new milestone created:

3a) Take sample in neighborhood of m Distance from m is initially p and shrinks

with each successive attempt

3b) If sample collision free, add it as child of m

SBL Algorithm - CONNECTSBL Algorithm - CONNECT1. m is new milestone2. m’ is other tree’s nearest milestone to m

SBL Algorithm - CONNECTSBL Algorithm - CONNECT3. If( distance(m, m’) < ρ )

3.1 Connect m and m’ by bridge

SBL Algorithm - CONNECTSBL Algorithm - CONNECT3. If( distance(m, m’) < ρ )

3.1 Connect m and m’ by bridge

…3.2 τ is path from start to goal3.3 Return results of TEST_PATH( τ )

SBL Algorithm - CONNECTSBL Algorithm - CONNECT

SBL Algorithm – SBL Algorithm – TEST_PATH(TEST_PATH(pathpath))Continually test “most unsafe” segment until

we we encounter a collision, or all segments are known to be safe. Save results for future use.

ResultsResults

Results for configuration shown in the figure

Performance Evaluation & Performance Evaluation & Convergence RateConvergence Rate

Comparative Performance Comparative Performance EvaluationEvaluationExperimental results for the full

collision-check planner

DiscussionDiscussionAssumes two spatially close configurations in

configuration space have low probability of collision

Saves time by checking collision between milestones only when part of candidate path from start to goal.

Valid assumption in practice & supported by experiments