Sampling Strategies By David Johnson

Probabilistic Roadmaps (PRM) [Kavraki, Svetska, Latombe, Overmars, 1996] start configuration goal configuration free-space c-obstacle Configuration-space components milestone local path Roadmap components

PRM planners solve complicated problems Complex geometries: obstacles: polygons Robot: 4053 polygons High dimensional

Main Issue: “Narrow Passages” free samples colliding samples colliding local path narrow passage low density of free samples high density of free samples The efficiency of PRM planners drops dramatically in spaces with narrow passages

Problems with “narrow passages” are commonly encountered Main Issue: “Narrow Passages”

PRM Sampling Random Free points can see a “lot” of free space –Somewhat invariant with dimensionality Not true for narrow passages Can we better characterize a narrow passge/difficult problem?

Coverage and Connectivity Coverage –How well does the roadmap approximate the configuration space Connectivity –Are regions of C-space connected that can be connected

Coverage BadGood

Coverage almost any point of the configuration space can be connected by a straight line segment to some milestone BadGood

Connectivity

1-1 correspondence between the connected components of the roadmap and those of F

Narrow Passages Connectivity is difficult to capture when there are narrow passages. How to characterize coverage/connectivity?Expansiveness a narrow passage is difficult to define.Easy Difficult

Visibility All configurations in Free Space that can be seen by a free configuration p p

Є-good Every free configuration “sees” at least a є fraction of the free space, є is in (0,1]. 0.5-good1-good The domain is only as good as its worst member F is 0.5-good

β-lookout of a subspace S Subset of points in S that can see at least a β fraction of F\S, β is in (0,1]. S F\S 0.4-lookout of S This area is about 40% of F\S S F\S 0.4-lookout of S

S F\S F is ε-good  ε=0.5 Definition: (ε,α,β)-expansive The free space F is (ε,α,β)-expansive if –Free space F is ε-good –For each subspace S of F, its β-lookout is at least α fraction of S. ε, α, β are in (0,1] β-lookout  β=0.4 Volume(β-lookout) Volume(S)  α=0.2 F is (ε, α, β)-expansive, where ε=0.5, α=0.2, β=0.4.

Why Expansive? ε, α, and β measure the expansiveness of a free space. Bigger ε, α, and β –Easier to construct a roadmap with good connectivity/coverage.

p3p3 pnpn P n+1 q p2p2 Linking sequence p p1p1 P n+1 is chosen from the lookout of the subset seen by p, p 1,…,p n Visibility of p Lookout of V(p)

Theorem 1 Probability of achieving good connectivity increases exponentially with the number of milestones (in an expansive space). As (ε, α, β) decrease  the number of milestones needs to be increased (to maintain good connectivity).

Theorem 2 Probability of achieving good coverage, increases exponentially with the number of milestones (in an expansive space).

Probabilistic Completeness In an expansive space, the probability that a PRM planner fails to find a path when one exists goes to 0 exponentially in the number of milestones (~ running time). [Hsu, Latombe, Motwani, 97]

Summary Main result –If a C-space is expansive, then a roadmap can be constructed efficiently with good connectivity and coverage. Limitation in implementation –No theoretical guidance about the stopping time. –A planner stops when either a path is found or Max steps have been taken.

Guided Sampling Motivation Two major costs of PRMs: –FREE - Check if sample point is in free space –JOIN – Check if path between milestones is in free space JOIN is 10 to 100 times slower than FREE Idea: selectively pick milestones –Try more samples ( n t ) –Keep fewer samples ( n a ) by filtering out non-promising samples Running time: T = n t T t + n a T a n t – milestones tried T t << T a n a – milestones added to graph

OBPRMs A randomized roadmap method for path and manipulation planning (Amato,Wu ICRA’96) OBPRM: An obstacle-based PRM for 3D workspaces (Amato,Bayazit, Dale, Jones and Vallejo)

Roadmap candidate points chosen on C- obstacle surfaces

Finding points on C-objects 1.Determine a point o inside s 2.Select m rays with origin o and directions uniformly distributed in C-space 3.For each ray identified above, use binary search to determine a point on s

Issues Selection of o in C- obstacle is crucial –To obtain uniform distribution of samples on the surface, would like to place origin somewhere near the center of C-object. – Still skewed objects would present a problem

Main Advantage Useful in manipulation planning where the robot has to move along contact surfaces Useful when C-space is very cluttered. On to the next ideas…

Two Similar Approaches The Gaussian Sampling Strategy for PRMs –Valerie Boor, Mark H. Overmars, A. Frank van der Stappen –ICRA 1999 The Bridge Test for Sampling Narrow Passages with PRMs –David Hsu, Tingting Jiang, John Reit, Zheng Sun –ICRA 2003

Overview Gaussian Strategy –Goals –Two Proposed Algorithms –Experimental Results Bridge Test –Proposed Algorithm –Comparison with Previous Paper –Experimental Results

Goal More samples in hard regions = more samples near obstacles

Parameters, Mixing Methods  indicates how close configurations are to obstacles  /2 Hybrid strategy: mix uniform sampling w/ Gaussian

Algorithm I loop –c 1 = random config. –d= distance sampled from Gaussian –c 2 = random config. distance d from c 1 –if Free(c 1 ) and !Free(c 2 ), add c 1 to graph –if Free(c 2 ) and !Free(c 1 ), add c 2 to graph Intuition: –Pick free points near blocked points –Avoid adding configurations in large empty regions hence the name C1C1 C2C2 d

Some Sampling Issues How to choose the offset point Make a random vector with components chosen from a Gaussian distribution –Bias towards the hypercube corners

Narrow Passage uniform sampling took 60 times longer than algorithm 1

Difficult Twist uniform sampling took 13 times longer than algorithm 1

Twisty Track uniform sampling took 4 times longer than algorithm 1

Overview Gaussian Strategy –Goals –Two Proposed Algorithms –Experimental Results Bridge Test –Proposed Algorithm –Comparison with Previous Paper –Experimental Results

Bridge Test loop –c 1 = random config. –if Free(c 1 ), continue (restart the loop) –d= distance sampled from Gaussian –c 2 = random config. distance d from c 1 –if Free(c 2 ), continue (restart the loop) –p = midpoint(c 1,c 2 ) –if Free(p), add p c1c1 p c2c2

Clover

Two Squares RBB SamplerGaussian Sampler

Depression, Zigzags

Bridge vs. Uniform RBB = Bridge

Conclusion Better configurations = fewer configurations = less edge computations = faster running time Gaussian –Points near obstacles –Points near two obstacles Bridge –Points between parts of obstacles Still un-tested in high-dimensional spaces

Medial-Axis Based PRM MAPRM: A Probabilistic Roadmap Planner with Sampling on the Medial Axis of the Free Space (Wilmarth, Amato, Stiller ICRA’99)

Main Ideas Beneficial to have samples on the medial axis; however, computation of medial axis itself is costly. Retraction : takes nodes from free and obstacle space onto the medial axis w/o explicit computation of the medial axis. This method increases the number of nodes found in a narrow corridor –independent of the volume of corridor –Depends on obstacles bounding it

Approach for Free-Space Find x o (nearest boundary point) for each point x in Free Space. Search along the ray x o x and find arbitrarily close points x a and x b s.t. x o is the nearest point on the boundary for x a but not x b. Called canonical retraction map

Extended Retraction Map Doing only for Free-Space => Only more clearance. Doesn’t increase samples in Narrow Passages Retract points that fall in C obstacle also. Retract points in the direction of the nearest boundary point

Results for 2D case LEFT: Helpful: obstacle-space that retracts to narrow passage is large RIGHT: Not Helpful: Obstacle-space seeping into medial axis in narrow corridor is very low

MAPRM for 3D rigid bodies

Example 2

Example 3

MAPRM for Point Robot in 2D [ Wilmarth, Amato, Stiller. ICRA’99 ] Clearance and penetration depth –The closest point on the polygon boundary clearance penetration

MAPRM for a Rigid Body in 3D [ Wilmarth, Amato, Stiller. SoCG’99 ] Clearance –The closest pair of points on the boundary of two polyhedra Penetration depth –If both polyhedra are convex Use Lin-Canny closest features algorithm [Lin and Canny ICRA ’ 99] –Otherwise Use brute force method [ Wilmarth, Amato, Stiller. SoCG’99 ] (test all possible pairs of features)

Approximate Variants of MAPRM Clearance and penetration depth –Both clearance and penetration depth are approximated –Following N random directions until collision status changes approximate MAPRM  approximateexact MAPRM  exact MAPRM Penetration Computation Clearance Computation Algorithm Rigid/articulated body General rigid body Convex rigid body Applied to Obstacle

What Else Can Be Tried?

Roadmap construction and repair fattened free space widened passage Fattening free space c-obstacle start goal Small-Step Retraction Method 1.Slightly fatten the robot’s free space 2.Construct a roadmap in fattened free space 3.Repair the roadmap into original free space (1) (2 & 3)

Small-Step Retraction Method Roadmap construction and repair fattened free space widened passage Fattening free space c-obstacle start goal -Free space can be “indirectly” fattened by reducing the scale of the geometries (usually of the robot) in the 3D workcell with respect to their medial axis -This can be pushed into the pre-processing phase

Small-Step Retraction Method Roadmap construction and repair fattened free space widened passage Fattening free space c-obstacle start goal Repair during construction Repair after construction goal Pessimist Strategy Optimist Strategy fattened free space start

Small-Step Retraction Method Roadmap construction and repair fattened free space widened passage Fattening free space c-obstacle start goal Repair during construction Repair after construction fattened free space goal Pessimist Strategy Optimist Strategy - Optimist may fail due to “false passages” but Pessimist is probabilistically complete - Hence Optimist is less reliable, but much faster due to its lazy strategy start

Small-Step Retraction Method Roadmap construction and repair fattened free space widened passage Fattening free space c-obstacle start goal Repair during construction Repair after construction goal Pessimist Strategy Optimist Strategy  Integrated planner: 1. Try Optimist for N time. 2. If Optimist fails, then run Pessimist fattened free space start

Quantitative Results Fattening “preserves” topology/ connectivity of the free space Fattening “alters” the topology/ connectivity of the free space Time SSRP (secs) Time SBL (secs) (a) (b) (c)2.141 (d) (e)65631 (f)13588> Time SSRP (secs) Time SBL (secs) (g) (h)3365> (a) (b) (c) (d) (e) (f) (g) (h) Alpha 1.0 Alpha 1.1 Upto 3 orders of magnitude improvement in the planning time was observed Our planner A recent PRM planner

Quantitative Results Test environments “without” narrow passages –SSRP and SBL have similar performance Time SSRP Time SBL (i) (j) (i) (j)

Conclusion SSRP is 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

“Visibility-based Probabilistic Roadmaps for Motion Planning” Siméon, Laumond, Nissoux from Mathieu Bredif

Motivation Save computation time without sacrificing coverage and connectivity. Visibility PRM is an optimized variation of basic PRM

Ensuring Coverage Visibility Domain of configuration q: q

Ensuring Coverage Free Space Coverage with Guard nodes Shared Visibility Guard

Ensuring Coverage Free Space Coverage with Guard nodes Shared Visibility Guard

Creating Connections Completing roadmap with Connection nodes Connection Shared Visibility Guard

Implementation Strategy

Termination criterion Meanwhile: 1/ntry = estimation of the volume of the subset of the free space not yet covered by visibility domains. M is max number of failures before the insertion of a new guard node. (1-1/M) = estimation of the coverage.

Visibility PRM – Basic PRM comparison Heavier Node “filtering” process BUT reduction in calls to the local method, from O(n 2 ) to O(n). Remaining problems of randomly chosen configuration points (inherent to PRM).

Conclusions Roadmap size is reduced. –Faster Queries –Few potential routes to choose from (good or bad?) Control of the coverage by (1-1/M) Computation Costs seem to be lower than with Basic-PRM.

Sampling Conclusions Directed sampling improves performance Clever techniques infer the presence of C- obstacles without explicit computation Still on-going development