star-shaped roadmaps gokul varadhan. prior work: motion planning complete planning –guaranteed to...

Star-Shaped Roadmaps

Gokul Varadhan

Prior Work: Motion Planning

• Complete planning – Guaranteed to find a path if one exists– Report non-existence otherwise

• Approximate planning

Prior Work: Complete Motion Planning

• General Methods– Exact cell decomposition

• [Schwartz & Sharir 83]• Originally, doubly exponential time in number of dofs• Recent results make it singly exponential [Basu,

Pollack and Roy 2003]

– Roadmap• [Canny 1988]• Singly exponential time in number of dofs

Prior Work: Complete Motion Planning

• Specific Methods– Planar objects

• [Kedem & Sharir 88; Avnaim & Boissonnat 89; Halperin & Sharir 96; Sacks 99; Flato & Halperin 2000]

– 3D Translation• Minkowski sum [Lozano-Perez 83]

– Convex objects• [Aronov & Sharir 94]• Voronoi diagram and retraction [Vleugels &

Overmars 97]

Prior Work: Motion Planning

• Approximate planning [Latombe 91]– Approximate cell decomposition – Potential field methods – Randomized sampling based methods

Completeness

• Under certain assumptions, these methods are

– Complete in a probabilistic sense

• Weak form of completeness

Issues

1. The planner may fail to find a path even if one exists

– “Narrow passage” problem– Many extensions have been proposed

• [Amato et al. 98; Hsu et al. 98; Hsu et al. 2003]

– No guarantees

2. It cannot handle path non-existence

Comparison

Completeness

Simplicity

Exact

methods

Randomized

Sampling Methods• Probabilistically complete• May not find paths

through narrow passages• Cannot handle path nonexistence

Goal

• Capture both– Completeness of the exact methods– Simplicity of sampling-based methods

• A complete sampling-based method

Main Results

• Star-shaped roadmaps – A new algorithm for complete motion

planning

– It captures the connectivity of the free space

– Can construct the roadmap using deterministic sampling

Outline

• Star-shaped Roadmaps

• Roadmap Construction– Deterministic sampling algorithm

• Results

• Limitations

• Comparison

Star-Shaped Property

• A region is star-shaped if there exists a point, called a guard, that can see every point in the region

o o

Star-Shaped Property and Path Planning

• Use the star-shaped property to capture the connectivity of a region

o

p q

Path between p and q is po :: oq

Overall Approach

• Use the star-shaped property to capture the local connectivity of the free space F

• Conceptually1. Decompose F into star-shaped regions

2. Intra-region connectivity captured by the guards

3. Inter-region connectivity captured by computing connectors

Star-Shaped Roadmap

1. Perform a star-shaped decomposition of free space

2. Compute connectors at the boundary between adjacent regions

1. Perform a star-shaped decomposition of free space

3. Construct the roadmap

2. Compute connectors at the boundary between adjacent regions

Motion Planning usingStar-Shaped Roadmap

p

q

Find a path between p and q1. Connect p and q to the roadmap along straight line paths to the guards (p and q resp)

2. Find a path between p and q by performing a graph search in the roadmap.

1. Connect p and q to the roadmap along straight line paths to the guards (p and q resp)

Path Non-Existence

pq

qpqp FR

Outline


• Roadmap Construction– Deterministic sampling algorithm

• Results

• Limitations

• Comparison

Star-Shaped Roadmap Construction

• We do not compute an explicit representation of F– Hence we cannot perform an explicit star-

shaped decomposition of F

• It is possible to construct a roadmap without explicit star-shaped decomposition

Deterministic Sampling

• We compute a – Subdivision of configuration space into

regions satisfying the star-shaped sampling condition

FR = F R is star-shaped

• Star-shaped Sampling Condition

–A region R satisfies the condition if

A

D C

B

Star-shaped Sampling

• Apply the star-shaped sampling condition recursively to perform adaptive subdivision


A B

CD

D

E

F

G

H

1. Compute a subdivision of the configuration space into regions R

such that FR is star-

shaped

Connector Computation

• Connector – A point that connects the free space of two

adjacent regions Ri and Rj if they are connected.

– It lies on the shared boundary Rij and belongs to F

Ri Rj

Rij


1. Compute a subdivision of the configuration space into regions R such that

FR is star-shaped

2. Compute connectors by applying a variant of Step 1in a lower dimension

1. Compute a subdivision of the configuration space into regions R such that

FR is star-shaped

3. Construct the roadmap

2. Compute connectors by applying a variant of Step 1in a lower dimension

Outline


• Roadmap Construction– Star-shaped Sampling– Star-shaped Test

• Results

• Limitations

• Comparison

Star-Shaped Test

• Consider two cases:– Linear primitive (polygon, polyhedron)– Nonlinear primitive

Star-shaped Test: Linear Primitive

• Reduces to linear programming

n

c

Linear constraint

n (c - p) > 0

p

Star-shaped Test: Linear Primitive

• Check if the linear program has a feasible solution

p

• Exact test is too expensive

• We use a conservative test

1. Estimate a candidate point

2. Verify if the primitive is indeed star-shaped w.r.t the candidate point

Star-shaped Test: Nonlinear Primitive

Star-Shaped Test

1. Candidate Point Estimation • Compute samples on the primitive• Perform linear programming

2. Verification• Use interval arithmetic

Preserves the correctness of the algorithm

Star-Shaped Test

• Given a region R, check if

FR = F R is star-shaped

• Free space is represented in terms of – Contact surfaces

Contact Surfaces

• Contact surfaces (C-surfaces) [Latombe 91]– A C-surface arises from a contact between features

of the robot and the obstacle• Portion of an algebraic surface

Ra1

b1

b2

O

Contact Surfaces

• F is bounded by the C-surfaces

C-surfaces

Free space F

F

C-obstacle

F

• Orient the C-surface

Intuitively, normal “points towards” C-obstacle

F

Contact Surface Condition

• Let denote the portion of C-surfaces that lie within a region R

o

C-obstacle

Contact surface

op np > 0

Is there a point o in the region R such that

for every point p in

op

C-surface condition

Free Space Existence

ooo

C-obstacle C-obstacle

Cell has a point in F o is in F

F

F

If C-surface condition holds

Star-shaped Test

oo

C-obstacle

FR is star-shaped w.r.t o

If C-surface condition holds and o is in F

F

Outline


• Roadmap Construction

• Results

• Limitations

• Comparison

Results

2GHZ Pentium IV with 512 MB memory

3T 3R2T+1R

2T+1R: Gears

Obstacles

Robot

2T+1R: Gears

Roadmap 112 secs

Path Search 0.17 secs

Start

Goal

2T+1R: Gears

2T+1R: GearsPath in Configuration Space

x

y

Goal

Start

Path

Narrow passage

RobotObstacle Obstacle

2T+1R: Gears

Start

Goal

2T+1R: Gears Path Non-Existence

No path exists!

Roadmap 115 secs


Start

Goal

3T: Assembly

ObstacleRobot

3T: Assembly

Roadmap 16 secs


Start

Goal

Obstacle

3T: Assembly

3T: AssemblyPath in Configuration Space

StartGoal

Path

3R Articulated Robot

Obstacle

Robot


Roadmap 9 secs


Start

Goal

3R Articulated RobotPath Non-Existence

No path exists!

Roadmap 9 secs


Roadmap

12 secsPath Search

0.1 secs

2T+1R: Maze

2T+1R: Gears

Obstacles

Robot

2T+1R: Gears

Roadmap 112 secs


Start

Goal

2T+1R: Gears

2T+1R: GearsFree Configuration Space Approximation

Goal

Start

Pathx

y

2T+1R: Gears

Start

Goal

2T+1R: Gears Path Non-Existence

No path exists!

Roadmap 115 secs


Start

Goal

3T: Assembly

ObstacleRobot

3T: Assembly

Roadmap 16 secs


Start

Goal

Obstacle

3T: Assembly

3T: Assembly Free Configuration Space Approximation

StartGoal

Path


Obstacle

Robot


Roadmap 9 secs


Start

Goal


No path exists!

Roadmap 9 secs


Outline


• Roadmap Construction

• Results

• Limitations

• Comparison

Degeneracies

• Star-shaped sampling condition will not be met in degenerate cases

Tangential contact

Narrow passage of width

zero

• Requires motion in contact space– Potential solution: Use the method by [Redon & Lin

2005] to do local planning in contact space

Limitation

• Sampling condition is conservative– May result in additional subdivision

Outline

• Star-shaped Roadmaps• Roadmap Construction• Results• Limitations• Comparison with prior methods

– Complete methods– Probabilistic roadmap (PRM) methods– Approximate cell decomposition– Visibility Based Methods

Overall Comparison

Complete

methods

PRM methods

Our method

Completeness

Simplicity

Probabilistically completeCannot handle path non-existence

Complete provided star-shaped sampling condition is metHandles path non-existence

Comparison

PRM Methods Star-shaped Roadmap Method

Difficult to implement for high dofEasily extends to very high dofs

Requires local planning No explicit local planning

High DOF

• Theory is general

• Curse of dimensionality

• Implementation complexity– Star-shaped test

• Uses linear programming & interval arithmetic• These extend to higher dimensions

– Difficult to enumerate the contact surfaces

Comparison

Approx Cell Decomp Star-shaped Roadmap Method

Resolution-complete Complete, provided the star-shaped sampling condition is met

Choosing a sufficient resolution is non-trivial.

Resolution determined by the sampling condition

Conservative approximation of F

Complete connectivity; guards cover every point in F

Cannot plan paths through mixed cell

Can plan paths through mixed regions

Comparison

Visibility PRM Star-shaped Roadmap Method

Objective is to generate a probabilistic roadmap with fewer nodes

Objective is to do complete planning

Randomized sampling Deterministic sampling

Computes inter-sample visibility

Star-shaped property defines the visibility of an entire region

[Simeon et al. 2000]

Main Results

1. Star-shaped roadmaps for complete motion planning

– Provides rigorous guarantees

Main Results

1. Star-shaped roadmaps for complete motion planning

– Provides rigorous guarantees

2. A deterministic sampling algorithm for roadmap construction

Conclusion

• Simple to implement for low dof

• Able to handle challenging scenarios– With narrow passages– No collision-free paths

Ongoing & Future Work

• Optimize the implementation– Reduce the number of contact surfaces

• Higher dofs– Rigid motion planning in 3D– 3T+3R

• Investigate combination with randomized and quasi-random [Branicky et al. 2001] sampling methods for high dof planning

Acknowledgement

• Shankar Krishnan

• Young J. Kim

• Ming Lin

• Members of UNC Gamma group

• Anonymous reviewers

Acknowledgement

• ARO Contracts

• NSF

• ONR

• DARPA

• Intel

Star-Shaped Roadmaps –

Gokul Varadhan

Dinesh Manocha

http://gamma.cs.unc.edu/motion

University of North Carolina at Chapel Hill

A Deterministic Sampling Approach for Complete Motion Planning

star-shaped roadmaps gokul varadhan. prior work: motion planning complete planning –guaranteed to...

Documents