Sampling Strategies

By David Johnson

Probabilistic Roadmaps (PRM)

[Kavraki, Svetska, Latombe, Overmars, 1996]

startconfiguration

goalconfiguration

free-spacec-obstacleConfiguration-space

components

milestonelocal path

Roadmapcomponents

PRM planners solve complicated problems

Complex geometries:obstacles: 43530 polygonsRobot: 4053 polygons

High dimensional

Main Issue: “Narrow Passages”

free samples

colliding samples colliding local path

narrow passagelow 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

Bad Good

Coverage

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

Bad Good

Connectivity

Bad Good

Connectivity

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

Bad Good

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.

EasyDifficult

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-good 1-good

The domain is onlyas 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.

p3

pn

Pn+1

q

p2

Linking sequence

p

p1

Pn+1 is chosen from the lookout of the subset seen by p, p1,…,pn

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 (nt )

– Keep fewer samples (na ) by filtering out non-promising

samples

Running time: T = nt Tt + na Ta

nt – milestones tried Tt << Ta

na – 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

– c1 = random config.– d = distance sampled from Gaussian– c2 = random config. distance d from c1

– if Free(c1) and !Free(c2), add c1 to graph– if Free(c2) and !Free(c1), add c2 to graph

• Intuition: – Pick free points near blocked points– Avoid adding configurations in large empty regions

hence the name

C1

C2

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

uniformsampling

took60 timeslongerthan

algorithm 1

Difficult Twist

uniformsampling

took13 timeslongerthan

algorithm 1

Twisty Track

uniformsampling

took4 timeslongerthan

algorithm 1

Overview

• Gaussian Strategy– Goals– Two Proposed Algorithms– Experimental Results

• Bridge Test– Proposed Algorithm– Comparison with Previous Paper– Experimental Results

Bridge Test

• loop– c1 = random config.

– if Free(c1), continue (restart the loop)

– d = distance sampled from Gaussian

– c2 = random config. distance d from c1

– if Free(c2), continue (restart the loop)

– p = midpoint(c1,c2)

– if Free(p), add pc1

p c2

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 xo (nearest boundary point) for each point x in Free Space.

• Search along the ray xox and find arbitrarily close points xa and xb s.t. xo is the nearest point on the boundary for xa

but not xb.

• 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 Cobstacle 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

approximateapproximateMAPRMapproximateexactMAPRMexactexactMAPRM

Penetration Computation

Clearance Computation

Algorithm

Rigid/articulated body

General rigid body

Convex rigid body

Applied to

Obstacle

What Else Can Be Tried?

Roadmap constructio

nand repair

fattened free space

widened passage

Fattening

free spacec-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 MethodRoadmap

construction

and repair

fattened free space

widened passage

Fattening

free spacec-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 MethodRoadmap

construction

and repair

fattened free space

widened passage

Fattening

free spacec-obstacle

start

goal

Repair during construction

Repair after construction

goal

PessimistStrategy

OptimistStrategy

fattenedfree space

start

Small-Step Retraction MethodRoadmap

construction

and repair

fattened free space

widened passage

Fattening

free spacec-obstacle

start

goal

Repair during construction

Repair after construction

fattenedfree space

goal

PessimistStrategy

OptimistStrategy

- 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 MethodRoadmap

construction

and repair

fattened free space

widened passage

Fattening

free spacec-obstacle

start

goal

Repair during construction

Repair after construction

goal

PessimistStrategy

OptimistStrategy

Integrated planner:

1. Try Optimist for N time. 2. If Optimist fails,

then run Pessimist

fattenedfree space

start

Quantitative Results• Fattening “preserves” topology/

connectivity of the free space• Fattening “alters” the topology/

connectivity of the free space

TimeSSRP(secs)

TimeSBL

(secs)

(a) 9.4 12295

(b) 32 5955

(c) 2.1 41

(d) 492 863

(e) 65 631

(f) 13588 >100000

TimeSSRP(secs)

TimeSBL

(secs)

(g) 386 572

(h) 3365 >100000

(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

TimeSSRP

TimeSBL

(i) 1.68 1.60

(j) 2.59 2.40

(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

SharedVisibility

Guard

Ensuring Coverage

• Free Space Coverage with Guard nodes

SharedVisibility

Guard

Creating Connections• Completing roadmap with Connection nodes

Connection

SharedVisibility

Guard

Implementation Strategy

Implementation Strategy

Implementation Strategy

Implementation Strategy

Implementation Strategy

Implementation Strategy

Implementation Strategy

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(n2) 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