Narrow Passage Problem in PRM

1. Advanced Robotic Presentation Narrow Passage Problemin PRM AmirhosseinHabibian Robotic Lab, University of Tehran

2. Outline • A brief overview on PRM • What’s Narrow Passage Problem? • Solutions of Narrow Passage Problem • Workspace-guided strategies • Filtering strategies • Adaptive strategies • Deformation strategies

3. A brief overview on PRM A brief overview on PRM • Basic idea of PRMs is: • Compute a very simplified representation of the free space by sampling configurations at random

4. A brief overview on PRM A brief overview on PRM • Basic PRM Algorithm: Input: geometry of the moving object & obstacles Output: roadmap G = (V, E) 1: V   and E  . 2:repeat 3: q a configuration sampled uniformly at random from C. 4: ifCLEAR(q)then 5:Add q to V. 6: Nq a set of nodes in V that are close to q. 6: for each q’ Nq 7: ifLINK(q’,q)then 8: Add an edge between q and q’ to E. CLEAR(q)Is configuration q collision free or not? LINK(q, q’) Is the straight-line path between q andq’ collision-free?

5. A brief overview on PRM A brief overview on PRM Why is PRM planning probabilistic? • PRM planner ignores the exact shape of Free Space. So, it acts like a robot building a map of an unknown environment with limited sensors • The goal is to minimize the expected number of remaining iterations to connect source to goal

6. A brief overview on PRM A brief overview on PRM Experimental Convergence Rate of Basic PRM: The graph plots the percentage of unsuccessful outcomes out of 100 independent runs for the same query

7. A brief overview on PRM A brief overview on PRM But sometimes Basic PRM doesn’t converge The plot shows the average running time for Basic PRM to connect the two query configurations q1 and q2, as the corridor width decreases.

8. A brief overview on PRM A brief overview on PRM Why? • If Free Space is • expansive, then Basic PRM answers planning queries correctly with high probability. • poorly expansive, then there exist queries for which we cannot expect Basic PRM to work well. Now we’ll see definition of expansiveness

9. A brief overview on PRM A brief overview on PRM Some definitions: • The visibility set of q є F is the set V(q), • V(q) = {q’ єF | FreePath(q, q’) is true} • Intuitively, є-good free space is a space in which every configuration q has a relatively large visibility set. • Let єbe a constant in (0, 1]. A point q єF is є-goodif it sees at least an є-fraction of F.

10. A brief overview on PRM A brief overview on PRM Some definitions: • β-LOOKOUT: Let F’ be a connected component of F and G be any subset of F’. Let βbe a constant in (0, 1]. The β-LOOKOUTof G is the set of all points in G such that each point sees at least a β-fraction of the complement of G: β-LOOKOUT(G) = {q є G | μ(V(q)\G) ≥ β*μ(F’\G)}.

11. A brief overview on PRM A brief overview on PRM Some definitions: • Let є, α, and β be constants in (0, 1]. A connected component F’ of F is • (є, α, β)-expansive, if • every point q є F’ is є-good • for any set M of points in F’ • μ(β-LOOKOUT(V(M))) ≥ α*μ(V(M)). • The free space F is (є, α, β)-expansive, if its connected components are all (є, α, β)-expansive.

12. Outline • A brief overview on PRM • What’s Narrow Passage Problem? • Solutions of Narrow Passage Problem • Workspace-guided strategies • Filtering strategies • Adaptive strategies • Deformation strategies

13. What’s Narrow Passage Problem? • What’s Narrow Passage Problem? Loosely speaking, narrow passages are small regions in free space, which are crucial and whose removal will changes the overall connectivity of the free space qinit qgoal Configuration Space

14. What’s Narrow Passage Problem? • What’s Narrow Passage Problem? • As mentioned, Narrow Passage occurs when the free space is poorly expansive. • In Narrow Passages: • For relatively large amount of є, most of the configurations aren’t є-good. • For relatively large amount of α, β the below relation isn’t consistent: • μ(β-LOOKOUT(V(M))) ≥ α*μ(V(M)). • So, when the free space is poorly expansive, we are faced with Narrow Passage Problem.

15. Outline • A brief overview on PRM • What’s Narrow Passage Problem? • Solutions of Narrow Passage Problem • Workspace-guided strategies • Filtering strategies • Adaptive strategies • Deformation strategies

16. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem Previous Works Index • Probabilistic Roadmap Methods • Uniform Sampling (original) [Kavraki, Latombe, Overmars, Svestka, 92, 94, 96] • Obstacle-based PRM (OBPRM) [Amato et al, 98] • PRM Roadmaps in Dilated Free space[Hsu et al, 98] • Gaussian Sampling PRMs[Boor/Overmars/van derSteppen 99] • PRM for Closed Chain Systems[Lavalle/Yakey/Kavraki 99, Han/Amato 00] • PRM for Flexible/Deformable Objects[Kavraki et al 98, Bayazit/Lien/Amato 01] • Visibility Roadmaps[Laumond et al 99] • Using Medial Axis [Kavraki et al 99, Lien/Thomas/Wilmarth/Amato/Stiller 99, 03, Lin et al 00] • Generating Contact Configurations[Xiao et al 99] • Single Shot [Vallejo/Remmler/Amato 01] • Bio-Applications: Protein Folding [Song/Thomas/Amato 01,02,03, Apaydin et al 01,02] • Lazy Evaluation Methods: [Nielsen/Kavraki 00 Bohlin/Kavraki 00, Song/Miller/Amato 01, 03] • Related Methods • Ariadnes Clew Algorithm [Ahuactzin et al, 92] • RRT (Rapidly Exploring Random Trees) [Lavalle/Kuffner 99]

17. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem How Important is the Sampling Method? • Basic PRM planner employ uniform distribution for sample point generation. • Most PRM planners employ non-uniform measures that dramatically improve Performance.

18. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem How Important is the Sampling Method? Comparison of three strategies with different sampling measures. The plot shows the average running time over 30 runs.

19. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem How to find the Sampling Method? • What is the source of information? • Robot and environment geometry • Previous generated Samples • How to exploit it? • Workspace-guided strategies • Filtering strategies • Adaptive strategies • Deformation strategies

20. Outline • A brief overview on PRM • What’s Narrow Passage Problem? • Solutions of Narrow Passage Problem • Workspace-guided strategies • Filtering strategies • Adaptive strategies • Deformation strategies • Solutions to Narrow Passage Problem

21. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Workspace-guided strategies Workspace-guided strategies • Narrow passages in Configuration space, are often caused by narrow passages in the workspace • So, we can find Narrow Passages by searching in workspace • Cell decomposition • Medial-axis transform • [J.P. van den Berg and M. H. Overmars. Using Workspace Information as a Guide to Non-Uniform Sampling in Probabilistic Roadmap Planners. IJRR, 24(12):1055-1071, Dec. 2005] • [H. Kurniawati and D. Hsu. Workspace importance sampling for probabilistic roadmap planning. In Proc. IEEE/RSJ Int. Conf. on Intelligent Robots & Systems, pp. 1618–1623, 2004]

22. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Workspace-guided strategies Workspace-guided strategies • Cell decomposition: • In this method, the environment is decomposed into black and white cells • Narrow Passages can be found regarding to this cells, e.g. by image processing algorithms • It is only applicable to configuration spaces with few dimensions

23. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Workspace-guided strategies • Cell decomposition Workspace-guided strategies Cell decomposition: A cell decomposition of the workspace of a two dimensional example scene

24. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Workspace-guided strategies Workspace-guided strategies • After finding Narrow Passages in workspace we can simply find them in Configuration Space, • e.g. Inverse Kinematic • Use this information to generate more sample points near to Narrow Passages

25. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Workspace-guided strategies Workspace-guided strategies Input: geometry of the moving object & obstacles Output: roadmap G = (V, E) 1: V   and E  . 2:repeat 3: NP = FIND //Find Narrow Passages in workspace 4: NP_Q = CONVERT(NP)//Convert NPs to Configuration Space 5: GENERATE(NP_Q) //Generate Sample points regarding to NP_Q 6: CONTINUE //Similar to BASIC_PRM

26. Outline • A brief overview on PRM • What’s Narrow Passage Problem? • Solutions of Narrow Passage Problem • Workspace-guided strategies • Filtering strategies • Adaptive strategies • Deformation strategies

27. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies Filtering strategies • FreePath, which checks the connection between two configurations, has much higher computational cost than FreeConf • Filtering increases the number of calls to FreeConf, but yields a smaller set of better placed roadmap nodes and thus reduces the number of calls to FreePath • It often leads to significant savings in computational time

28. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies Filtering strategies 1: WHILE TRUE 2: Sample a Configuration in the same region 3:IF generated sample follows desired pattern 4: Retain generated sample as a milestone 5: ELSE 6: Discard generated sample 7: END WHILE • More sampling work, but better distribution of nodes • Less time is wasted in connecting nodes

29. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies Filtering strategies • Some Filtering Strategy Sampling method: • Gaussian Sampling • Bridge Test • Hybrid

30. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies • Gaussian Sampling Gaussian Sampling 1: Sample a configuration q uniformly at random from configuration space 2: Sample a real number x at random with Gaussian distribution N[0,s](x) 3: Sample a configuration q’ in the ball B(q,|x|) uniformly at random 4: IF only one of q and q’ is in free space 5: retain the one in free space as a node 6: ELSE 7: retain none The gain is not in sampling fewer milestones, but in connecting fewer pairs of milestones

31. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies • Gaussian Sampling Uniform vs. Gaussian Sampling 13,000 Milestones created by uniform sampling before the narrow passage was adequately sampled 150 Milestones created by Gaussian sampling

32. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies • Gaussian Sampling Gaussian Sampling • This strategy tries to locate the boundary of Free Space and sample more densely there • The rationale is that configurations with poor visibility often lie close to the boundary of Free Space

33. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies • Bridge Test Bridge Test 1: Sample two conformations q and q’ using Gaussian sampling technique 2: If none is in free space 3: IF qm = (q+q’)/2 is in free space 4: retain qm as a milestone 5: ELSE 6: retain none

34. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies • Bridge Test Bridge Test • Main idea is: “Building short bridges is much easier in narrow passages than in wide-open free space” Bridge test Gaussian

35. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies • Bridge Test Bridge Test • Bridge Test tries to capture a different kind of geometric pattern

36. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies • Hybrid Hybrid 1: Sample two configurations q and q’ using Gaussian sampling technique 2: IF both are in free space 3: retain one (any of the two) as a node with low probability (e.g., 0.1) 4: ELSE IF only one is in free space 5: retain it as a node with intermediate probability (e.g., 0.5) 6: ELSE IF qm = (q+q’)/2 is in free space 7: retain it as a node with high probability (e.g., 1)

37. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Filtering strategies • Hybrid Hybrid Uniform Sampling Bridge Test Hybrid Sampling

38. Outline • A brief overview on PRM • What’s Narrow Passage Problem? • Solutions of Narrow Passage Problem • Workspace-guided strategies • Filtering strategies • Adaptive strategies • Deformation strategies

39. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Adaptive strategies Adaptive strategies • Use intermediate sampling results to identify regions of the free space whose connectivity is more difficult to capture • Greater density of milestones in “difficult” regions of the feasible space

40. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Adaptive strategies Adaptive strategies • Two-phase connectivity expansion • Tree expansion • Unsupervised on-line learning

41. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Adaptive strategies • Two-phase Adaptive strategies Two-phase connectivity expansion: 1: Construct initial PRM with uniform sampling 2: Identify milestones that have few connections to their close neighbors \* Performed by counting the numbers of successful and unsuccessful connections of nodes while generation of random samples*\ 3: Sample more configurations around them

42. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Adaptive strategies • Two-phase Adaptive strategies Two-phase connectivity expansion: the Gaussian strategy the two-phase connectivity expansion strategy

43. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Adaptive strategies • Tree Expansion Adaptive strategies • Tree expansion: • Try to expand each portion of Free Space, independently • hypothesize the location of the boundary of the portion of Free Space represented by the current roadmap • In each sampling step, try to expand this boundary by sampling new configurations around a node of the roadmap believed to be close to the boundary

44. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Adaptive strategies • Tree Expansion Adaptive strategies • Tree expansion: • Probability measure for sampling a new configuration is conditioned on the existing roadmap nodes • Automatically adapts over time • Do not intentionally try to sample more densely in regions with poor visibility

45. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Adaptive strategies • Unsupervised Adaptive strategies • Unsupervised on-line learning: • Closest form to the strategy for constructing optimal sampling measures • Creates and updates an approximate model of Free Space in the form of a collection of Gaussian functions • Uses this model to sample configurations so that the expected value of a utility function is maximized

46. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Adaptive strategies • Unsupervised Adaptive strategies • Unsupervised on-line learning: • Works as Expectation Maximization Optimization algorithm • Has a quasi static approach

47. Outline • A brief overview on PRM • What’s Narrow Passage Problem? • Solutions of Narrow Passage Problem • Workspace-guided strategies • Filtering strategies • Adaptive strategies • Deformation strategies

48. difficult narrow passages easy narrow passages Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Deformation strategies Deformation strategies We observed that slightly widening difficult narrow passages dramatically improves the efficiency of PRM planning planning time decreasing width of the narrow passage

49. Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Deformation strategies Deformation strategies • Main Idea: • Deform the free space to make it more expansive • Method: • Free space dilatation

50. start Fattening goal widened passage c-obstacle fattened free space free space Solutions to Narrow Passage Problem • Solutions to Narrow Passage Problem • Deformation strategies • Free space dilatation Deformation strategies Roadmap construction and repair (2 & 3) (1) • Slightly fatten the robot’s free space • Construct a roadmap in fattened free space • Repair the roadmap into original free space