Star-Shaped Roadmaps

1
Star-Shaped Roadmaps

• Gokul Varadhan

2
Prior Work Motion Planning

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

3
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

4
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

5
Prior Work Motion Planning

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

6
Completeness

• Under certain assumptions, these methods are
• Complete in a probabilistic sense
• Weak form of completeness

7
Issues

• 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
• It cannot handle path non-existence

8
Comparison
Exact methods Randomized Sampling Methods

• Probabilistically complete
• May not find paths through narrow passages
• Cannot handle path
• nonexistence

Completeness
Simplicity
9
Goal

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

• A complete sampling-based method

10
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

11
Outline

• Star-shaped Roadmaps
• Roadmap Construction
• Deterministic sampling algorithm
• Results
• Limitations
• Comparison

12
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
13
Star-Shaped Property and Path Planning

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

Path between p and q is po oq
o
p
q
14
Overall Approach

• Use the star-shaped property to capture the local
  connectivity of the free space F
• Conceptually
• Decompose F into star-shaped regions
• Intra-region connectivity captured by the guards
• Inter-region connectivity captured by computing
  connectors

15
Star-Shaped Roadmap
16
Motion Planning usingStar-Shaped Roadmap
Find a path between p and q
p
q
17
Complete Planning
R is the star-shaped roadmap
p
q
18
Path Non-Existence
p
q
19
Outline

• Star-shaped Roadmaps
• Roadmap Construction
• Deterministic sampling algorithm
• Results
• Limitations
• Comparison

20
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

21
Deterministic Sampling

• We compute a
• Subdivision of configuration space into regions
  satisfying the star-shaped sampling condition

A

• Star-shaped Sampling Condition
• A region R satisfies the condition if

B
FR F ? R is star-shaped
D
C
22
Star-shaped Sampling

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

23
Star-shaped Sampling
A
B

1. Compute a subdivision of the configuration space
   into regions R such that FR is star-shaped

D
C
24
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

Rij
Rj
Ri
25
Star-shaped Sampling

• Compute a subdivision of
• the configuration space
• into regions R such that
• FR is star-shaped

26
Outline

• Star-shaped Roadmaps
• Roadmap Construction
• Star-shaped Sampling
• Star-shaped Test
• Results
• Limitations
• Comparison

27
Star-Shaped Test

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

28
Star-shaped Test Linear Primitive

• Reduces to linear programming

Linear constraint
n (c - p) gt 0
n
c
p
29
Star-shaped Test Linear Primitive

• Check if the linear program has a feasible
  solution

p
30
Star-shaped Test Nonlinear Primitive

• Exact test is too expensive
• We use a conservative test
• Estimate a candidate point
• Verify if the primitive is indeed star-shaped
  w.r.t the candidate point

31
Star-Shaped Test

• Candidate Point Estimation
• Compute samples on the primitive
• Perform linear programming
• Verification
• Use interval arithmetic

Preserves the correctness of the algorithm
32
Star-Shaped Test

• Given a region R, check if
• FR F ? R is star-shaped
• Free space is represented in terms of
• Contact surfaces

33
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

b2
R
O
a1
b1
34
Contact Surfaces

• F is bounded by the C-surfaces

C-surfaces
F
C-obstacle
35
Contact Surface Condition

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

Contact surface
o
C-obstacle
36
Free Space Existence
If C-surface condition holds
Cell has a point in F ? o is in F
?F
o
o
o
?F
C-obstacle
C-obstacle
37
Star-shaped Test
If C-surface condition holds and o is in F
FR is star-shaped w.r.t o
o
o
?F
C-obstacle
38
Outline

• Star-shaped Roadmaps
• Roadmap Construction
• Results
• Limitations
• Comparison

39
Results
3T
3R
2T1R
2GHZ Pentium IV with 512 MB memory
40
2T1R Gears
Obstacles
Robot
41
2T1R Gears
Goal
Start
Roadmap 112 secs
Path Search 0.17 secs
42
2T1R Gears
43
2T1R GearsPath in Configuration Space
Path
Goal
Start
44
2T1R Gears
Goal
Start
45
2T1R Gears Path Non-Existence
Goal
Start
46
3T Assembly
Robot
Obstacle
47
3T Assembly
Obstacle
Start
Goal
Roadmap 16 secs
Path Search 0.22 secs
48
3T Assembly
49
3T AssemblyPath in Configuration Space
50
3R Articulated Robot
Robot
Obstacle
51
3R Articulated Robot
Start
Goal
Roadmap 9 secs
Path Search 0.2 secs
52
3R Articulated Robot
53
3R Articulated Robot
54
3R Articulated RobotPath Non-Existence
55
2T1R Maze
Roadmap 12 secs
Path Search 0.1 secs
56
2T1R Gears
Obstacles
Robot
57
2T1R Gears
Goal
Start
Roadmap 112 secs
Path Search 0.17 secs
58
2T1R Gears
59
2T1R GearsFree Configuration Space
Approximation
60
2T1R Gears
Goal
Start
61
2T1R Gears Path Non-Existence
Goal
Start
62
3T Assembly
Robot
Obstacle
63
3T Assembly
Obstacle
Start
Goal
Roadmap 16 secs
Path Search 0.22 secs
64
3T Assembly
65
3T Assembly Free Configuration Space
Approximation
66
3R Articulated Robot
Robot
Obstacle
67
3R Articulated Robot
Start
Goal
Roadmap 9 secs
Path Search 0.2 secs
68
3R Articulated Robot
69
3R Articulated Robot
70
3R Articulated RobotPath Non-Existence
71
3R Articulated RobotPath Non-Existence
72
3R Articulated RobotPath Non-Existence
73
Outline

• Star-shaped Roadmaps
• Roadmap Construction
• Results
• Limitations
• Comparison

74
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

75
Limitation

• Sampling condition is conservative
• May result in additional subdivision

76
Outline

• Star-shaped Roadmaps
• Roadmap Construction
• Results
• Limitations
• Comparison with prior methods
• Complete methods
• Probabilistic roadmap (PRM) methods
• Approximate cell decomposition
• Visibility Based Methods

77
Overall Comparison
Probabilistically complete Cannot handle path
non-existence
Complete provided star-shaped sampling
condition is met Handles path non-existence
Complete methods PRM methods Our method
Completeness
Simplicity
78
Comparison
PRM Methods Star-shaped Roadmap Method


Requires local planning
No explicit local planning
Difficult to implement for high dof
Easily extends to very high dofs
79
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

80
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
81
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
82
Main Results

• Star-shaped roadmaps for complete motion planning
• Provides rigorous guarantees

83
Main Results

• Star-shaped roadmaps for complete motion planning
• Provides rigorous guarantees
• A deterministic sampling algorithm for roadmap
  construction

84
Conclusion

• Simple to implement for low dof
• Able to handle challenging scenarios
• With narrow passages
• No collision-free paths

85
Ongoing Future Work

• Optimize the implementation
• Reduce the number of contact surfaces
• Higher dofs
• Rigid motion planning in 3D
• 3T3R
• Investigate combination with randomized and
  quasi-random Branicky et al. 2001 sampling
  methods for high dof planning

86
Acknowledgement

• Shankar Krishnan
• Young J. Kim
• Ming Lin
• Members of UNC Gamma group
• Anonymous reviewers

87
Acknowledgement

• ARO Contracts
• NSF
• ONR
• DARPA
• Intel

88
Star-Shaped Roadmaps
A Deterministic Sampling Approach for Complete
Motion Planning

• Gokul Varadhan
• Dinesh Manocha

University of North Carolina at Chapel Hill
http//gamma.cs.unc.edu/motion
89
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

Rij
Rj
Ri
90
Connector Computation

• Compute a subdivision of Rij into regions Q
  satisfying the star-shaped condition in one lower
  dimension, i.e. FQ F ? Q is star-shaped
• If any of the guards in Q lie in F then
• Use it as a connector
• If none of the guards in Q lie in F then
• The free space in Ri and Rj are not connected
• No connector exists

91
Comparison with Prior Work

• Compared to our prior work (WAFR 2004), current
  approach is
• Simpler
• Less conservative subdivision
• Extensible to higher dimensional configuration
  spaces
• Less prone to degeneracies

92
C-Constraint
a2
b2
R
O
a1
n
a0
b1
(a0 - a1) . n gt 0
(a1 - b1) . n lt 0
gt

(a2 - a1) . n gt 0
93
Narrow Passage Problem
94
Non-Existence Problem
95
Comparison with Prior Work

• PRMs Kavraki et al. 1994, Visibility PRMs
  Simeon et al. 1999
• Applicable to very high DOFs
• Probabilistically complete
• Quasi-Randomized Sampling Branicky et al. 2001
• Applicable to very high DOFs
• Resolution-complete

96
TODO

• PRM extensions
• Do better in many situations but not guarantees
• Stats for 3R robot
• Add another image for path non-existence
• Show an example 3D star-shaped roadmap

97
2D Example Contact Surfaces
98
2D Example Contact Surfaces
A
B
C
99
2D Example Contact Surfaces
F
100
Star-Shaped Test Boolean Combination

• If the primitives are linear
• Combine the linear constraints of all the
  primitives
• Use linear programming
• If the primitives are non-linear
• Sample all the primitives
• Use Step (1) to estimate a candidate point
• Verify if every primitive is star-shaped w.r.t
  the candidate point
• Works for a combination of linear and non-linear
  primitives as well

101
Comparison
Prior Sampling Based Approach Star-shaped Roadmap Approach
Compute samples uniformly or randomly Compute guards and connectors deterministically using star-shaped sampling
Check if they are in free space The guards and connectors are in free space by construction
Do local planning between nearby samples No explicit local planning star-shaped property guarantees local collision-free paths
102
Star-Shaped Test Boolean Combination

• If both A and B are star-shaped w.r.t a common
  point p, then so are and

A
B
p
103
Comparison
Approx Cell Decomp Star-shaped Roadmap Approach
Decomposition into empty, full and mixed cells Decomposition into regions satisfying star-shaped property
Conservative approximation of F Complete connectivity every point in F is captured implicitly
Need to subdivide mixed cells Not necessary to subdivide mixed regions that satisfy the star-shaped property
Check for paths through empty cells and not mixed cells Check for paths through empty regions as well as mixed regions that satisfy the star-shaped property
104
Probabilistic Roadmap (PRM)
free space
Kavraki, Svetska, Latombe,Overmars, 96
105
Main Results

• Current work focused on robots with low degrees
  of freedom (dofs)
• 2T1R
• A planar rigid robot capable of translation as
  well as rotation
• 3T
• A 3D rigid robot capable of translation
• 3R
• A planar articulated robot with 3 revolute joints

106
Issues

• Narrow passage problem
• Planner may not find a path even if a valid path
  exists
• Especially through narrow passages

107
Issues

• Narrow passage problem
• Planner may not find a path even if a valid path
  exists
• Especially through narrow passages
• Does not detect non-existence of a collision-free
  path

108
Star-Shaped Test

• Linear programming and interval arithmetic
• Standard techniques
• Extend easily to higher dimensions

109
Star-shaped Test

• Given a region R, check if
• FR F ? R is star-shaped
• Does not require an explicit computation of F

110
Path Length

• Basic algorithm does not optimize the path length
• Possible to bound the path length by adding an
  additional criterion
• All grid cells be smaller than some threshold ?

111
Issues

• PRM methods are probabilistically-complete
• If a path exists,
• As the number of samples increases, the
  probability of finding a path approaches 1.

112
Star-Shaped Roadmap Construction

• Our method requires a star-shaped decomposition
  of F
• Issue
• In practice, not possible to compute such a
  decomposition explicitly
• Do not have an explicit representation of F
• We compute a star-shaped decomposition implicitly

113
Visibility PRM

• Visibility PRM method Simeon et al. 2000
• Uses visibility to compute guards and connectors
• Computes inter-sample visibility
• Uses randomized sampling
• Objective is to construct a roadmap with fewer
  nodes

114
Comparison
Complete methods Randomized Sampling Methods

• Probabilistically complete
• Cannot handle path
• nonexistence

Completeness
Simplicity
Efficiency
115
Overall Comparison
Probabilistically complete Cannot handle path
non-existence
Complete provided star-shaped sampling
condition is met Handles path non-existence
Complete methods PRM methods Our method
Completeness
Simplicity
Efficiency
116
2T1R GearsConfiguration Space
Contact Surfaces
Path
Goal
Start
117
Assembly Configuration Space
Contact Surfaces
Goal
Start
Path
118
Candidate Point Estimation

• 1. Compute samples on the nonlinear primitive.

2. Each sample defines a linear constraint.

1. Do linear programming

• If a feasible point exists, use it as a candidate
  point
• Varadhan et al. 2004

119
Verification

• A surface S is star-shaped w.r.t a point p if
• n (x p) gt 0 ? x ?S
• where n is the normal at point x

p
x
n
120
Interval Arithmetic

• Use interval arithmetic to test if
• n (x p) gt 0 ? x ?S
• where n is the normal at point x

121
Configuration Space Approximation
Free Space Approximation Maze Example
y
?
x
12 secs 1,550 contact surfaces
122
Our Method

• Our method is different in the following respect
  
• We compute the guards and connectors
• In configuration space
• Deterministically
• Without an explicit computation of the free space

123
2T1R GearsFree Configuration Space
Approximation
?
Path
y
Goal
x
Start
124
Related Visibility Based Methods

• Art Gallery Problem Rourke 1987
• Goal is to compute a minimum set of guards
• In our context, this is not necessary

• Visibility graph method
• Nilsson, 1969, Laumond, 1987
• Visibility sets
• Used in the analysis of PRM methods Barraquand
  et al. 1997 Hsu et al. 1999

• Visibility Based Pursuit Evasion
• Suzuki Yamashita 1992,
• LaValle et al. 1997