Narrow Passage Problem in PRM Amirhossein Habibian Robotic Lab, University of Tehran Advanced Robotic Presentation

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

A brief overview on PRM Basic idea of PRMs is: –Compute a very simplified representation of the free space by sampling configurations at random 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: if CLEAR(q)then 5: Add q to V. 6: N q  a set of nodes in V that are close to q. 6: for each q’  N q 7: if LINK(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 and q’ collision-free ? A brief overview on PRM

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 Why is PRM planning probabilistic? 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 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. 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 A brief overview on PRM

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 є-good if it sees at least an є-fraction of F. Some definitions: 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 β-LOOKOUT of 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)}. 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. A brief overview on PRM

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

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 q init q goal Configuration Space 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. What’s Narrow Passage Problem?

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 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 der Steppen 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] Previous Works Index Solutions to Narrow Passage Problem

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

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

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 How to find the Sampling Method? Solutions to Narrow Passage Problem

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

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): , 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] Workspace-guided strategies Solutions to Narrow Passage Problem Workspace-guided strategies

Solutions to Narrow Passage Problem 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 Workspace-guided strategies Solutions to Narrow Passage Problem Workspace-guided strategies

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

Solutions to Narrow Passage Problem 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 Workspace-guided strategies Solutions to Narrow Passage Problem Workspace-guided strategies

Solutions to Narrow Passage Problem 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 Workspace-guided strategies Solutions to Narrow Passage Problem Workspace-guided strategies

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 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 Solutions to Narrow Passage Problem Filtering strategies

Solutions to Narrow Passage Problem 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 Solutions to Narrow Passage Problem Filtering strategies

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

Solutions to Narrow Passage Problem 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 Solutions to Narrow Passage Problem Filtering strategies Gaussian Sampling

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

Solutions to Narrow Passage Problem 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 Solutions to Narrow Passage Problem Filtering strategies Gaussian Sampling

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

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

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

Solutions to Narrow Passage Problem 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 q m = (q+q’)/2 is in free space 7:retain it as a node with high probability (e.g., 1) Solutions to Narrow Passage Problem Filtering strategies Hybrid

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

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 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 Solutions to Narrow Passage Problem Adaptive strategies

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

Solutions to Narrow Passage Problem 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 Solutions to Narrow Passage Problem Adaptive strategies Two-phase

Solutions to Narrow Passage Problem Adaptive strategies Two-phase connectivity expansion: (a)the Gaussian strategy (b)the two-phase connectivity expansion strategy Solutions to Narrow Passage Problem Adaptive strategies Two-phase

Solutions to Narrow Passage Problem Adaptive strategies 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 Tree expansion: Solutions to Narrow Passage Problem Adaptive strategies Tree Expansion

Solutions to Narrow Passage Problem Adaptive strategies 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 Tree expansion: Solutions to Narrow Passage Problem Adaptive strategies Tree Expansion

Solutions to Narrow Passage Problem 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 Solutions to Narrow Passage Problem Adaptive strategies Unsupervised

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

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 Deformation strategies We observed that slightly widening difficult narrow passages dramatically improves the efficiency of PRM planning decreasing width of the narrow passage planning time easy narrow passages difficult narrow passages Solutions to Narrow Passage Problem Deformation strategies

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

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

Solutions to Narrow Passage Problem Deformation strategies Free space can be “indirectly” fattened by reducing the scale of the geometries in the 3D workcell with respect to their medial axis This can be pushed into the pre-processing phase Solutions to Narrow Passage Problem Deformation strategies Free space dilatation

Solutions to Narrow Passage Problem Deformation strategies Deformation strategies are very efficient at finding Narrow Passages and still works well when there is none The main drawback is that there is an additional pre-computation step F A milestone with small penetration Solutions to Narrow Passage Problem Deformation strategies Free space dilatation

Questions ?

References All references and other useful materials are available in: Main references: 1. On the Probabilistic Foundations of Probabilistic Roadmap Planning, David Hsu, Jean-Claude Latombe, Hanna Kurniawati 2. Using Workspace Information as a Guide to Non-uniform Sampling in Probabilistic Roadmap Planners, Jur P. van den Berg, Mark H. Overmars 3. The Gaussian Sampling Strategy for Probabilistic Roadmap Planners, Valkrie Boor, Mark H. Overmars, A. Frank van der Stappen 4. The Bridge Test for Sampling Narrow Passages with Probabilistic Roadmap Planners 5. Smoothed Analysis of Probabilistic Roadmaps, Siddhartha Chaudhuri,Vladlen Koltun