Workspace-based Connectivity Oracle

An Adaptive Sampling Strategy for PRM Planning

Hanna Kurniawati and David HsuPresented by Nicolas Lee and Stephen Russell

Outline

• Introduction/Motivation

• WCO Planner

• Constructing a component sampler

• Ensemble sampler

• Results

Introduction

• Standard Probabilistic Road Map (PRM)– Two phases: construction and query– Construction creates map, R, that tries to

accurately model connectivity of C– Query tries to connect start/goal locations to R

Motivation

• Performance depends on quality of R– Coverage and connectivity

• Algorithm struggles with narrow passages in C• Other sampling strategies:

– Dynamic: Machine learning/adaptive hybrid– Workspace information: Identifying important regions

in W• e.g. Workspace Importance Sampling (WIS) focuses on

regions with small local feature size

WCO Foundations

• Proposition:

If two configurations q, q’ є C are connected by a path in Fc , then for any point f in a robot, Pf(q) and Pf(q’), the projections of q and q’ in W, are connected by a path in Fw

WCO

• Distinct components of R may in fact lie in the same connected component of Fc

• Examine workspace paths for multiple feature points and construct sampler for each f

• Search for channels in W and adapt distribution to sample more densely in regions covered by these channels

Workspace Connectivity

• Decomposition T of Fw into non-overlapping cells– Create adjacency grid GT of T

• Consider two milestones, m and m’, and projections onto W, Pf(m) є t and Pf(m’) є t’

• Find workspace channel, λ: set of nodes in GT connecting t and t’

• Lf( λ) suggests a region of Fc for sampling

Example

(a) Milestones projected to decomposed workspace

(b) Adjacency graph GT

(c) Channel graph G’

Component Sampler Algorithm1. Given f, sample configuration q based on sampling distribution over

T2. If q is collision free, then3. Insert q into R as new milestone m4. Nm, set of neighbors5. for each m’ є Nm do6. if m є Ri and m’ є Rj, then7. connect if possible8. Project m to W9. Update label sets for affected T10. Delete paths in G’ connecting terminals with same label set11. Let t є T containing Pf(m). Perform breadth-first search and

stop when reaching first terminal t’ ≠ t12. Add path from t→t’ to G’ if they have different label sets13. Update the sampling distribution

Ensemble Sampler Algorithm

1. Initialize pi = 1/K for i = 0, 1, …, K-12. for t = 1, 2, … do3. Pick a component sampler si with probability pi

4. Sample a new configuration q using the component

sampler picked5. If a new milestone m is added to the roadmap R

then6. Update the distribution for each

component sampler si

7. Update the probabilities pi

Probability Update

Ktw

twp

iKi

ii

)(

)()1(

10

Kp

rtwtw

iii

exp)()1(

• Ensemble sampler performs almost as well as the best component sampler

• Kinematic constraints taken into account through higher probability in overlapping lifted channels

Choosing Feature Points

• Must be representative of the robot

• Use vertices of convex hull and centroid for each rigid link of a robot

Test Configurations

Comparison With Other Samplers

• WCO has better sampling in channel regions without too many samples elsewhere

• In many cases, run time is cut in half compared to the best of the other three samplers

Limitations - 2 Bars Example

Conclusion

• WCO is an adaptive sampling strategy for PRM planning

• Using AHS, combine information from workspace geometry and sampling history

• In trials, WCO outperformed strategies which only use workspace information OR dynamic sampling