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

8. Comparison Completeness Simplicity Exact methods Randomized Sampling Methods Probabilistically complete May not find paths through narrow passages Cannot handle path nonexistence

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 o p q Path between p and q is po :: oq

14. Overall Approach Use the star-shaped property to capture the local connectivity of the free space F Conceptually 1.Decompose F into star-shaped regions 2.Intra-region connectivity captured by the guards 3.Inter-region connectivity captured by computing connectors

15. 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

16. Motion Planning using Star-Shaped Roadmap p q Find a path between p and q 1.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)

17. Complete Planning  p q R is the star-shaped roadmap

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 F R = F  R is star-shaped Star-shaped Sampling Condition –A region R satisfies the condition if A DC B

22. Star-shaped Sampling Apply the star-shaped sampling condition recursively to perform adaptive subdivision

23. Star-shaped Sampling A B C D D E F G H 1.Compute a subdivision of the configuration space into regions R such that F R is star- shaped

24. Connector Computation Connector –A point that connects the free space of two adjacent regions R i and R j if they are connected. –It lies on the shared boundary R ij and belongs to F RiRi RjRj R ij

25. Star-shaped Sampling 1.Compute a subdivision of the configuration space into regions R such that F R 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 F R is star-shaped 3. Construct the roadmap 2.Compute connectors by applying a variant of Step 1in a lower dimension

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 n c Linear constraint n (c - p) > 0 p

29. Star-shaped Test: Linear Primitive Check if the linear program has a feasible solution p

30. 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

31. 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

32. Star-Shaped Test Given a region R, check if F R = 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 R a1a1 b1b1 b2b2 O

34. 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F

35. Contact Surface Condition Let  denote the portion of C-surfaces that lie within a region R o C-obstacle Contact surface op  n p > 0 Is there a point o in the region R such that for every point p in  o p C-surface condition

36. Free Space Existence o o o C-obstacle Cell has a point in F  o is in F FF FF If C-surface condition holds

37. Star-shaped Test o o C-obstacle F R is star-shaped w.r.t o If C-surface condition holds and o is in F FF

38. Outline Star-shaped Roadmaps Roadmap Construction Results Limitations Comparison

39. Results 2GHZ Pentium IV with 512 MB memory 3T3R 2T+1R

40. 2T+1R: Gears Obstacles Robot

41. 2T+1R: Gears Roadmap 112 secs Path Search 0.17 secs Start Goal

42. 2T+1R: Gears

43. 2T+1R: Gears Path in Configuration Space x y  Goal Start Path Narrow passage Robot Obstacle

44. 2T+1R: Gears Start Goal

45. 2T+1R: Gears Path Non-Existence No path exists! Roadmap 115 secs Path Search 0.18 secs Start Goal

46. 3T: Assembly Obstacle Robot

47. 3T: Assembly Roadmap 16 secs Path Search 0.22 secs Start Goal Obstacle

48. 3T: Assembly

49. 3T: Assembly Path in Configuration Space Start Goal Path

50. 3R Articulated Robot Obstacle Robot

51. 3R Articulated Robot Roadmap 9 secs Path Search 0.2 secs Start Goal

52. 3R Articulated Robot

54. 3R Articulated Robot Path Non-Existence No path exists! Roadmap 9 secs Path Search 0.18 secs

55. Roadmap 12 secs Path Search 0.1 secs 2T+1R: Maze

56. 2T+1R: Gears Obstacles Robot

57. 2T+1R: Gears Roadmap 112 secs Path Search 0.17 secs Start Goal

58. 2T+1R: Gears

59. 2T+1R: Gears Free Configuration Space Approximation Goal Start Path x y 

60. 2T+1R: Gears Start Goal

61. 2T+1R: Gears Path Non-Existence No path exists! Roadmap 115 secs Path Search 0.18 secs Start Goal

62. 3T: Assembly Obstacle Robot

63. 3T: Assembly Roadmap 16 secs Path Search 0.22 secs Start Goal Obstacle

64. 3T: Assembly

65. 3T: Assembly Free Configuration Space Approximation Start Goal Path

66. 3R Articulated Robot Obstacle Robot

67. 3R Articulated Robot Roadmap 9 secs Path Search 0.2 secs Start Goal

68. 3R Articulated Robot

70. 3R Articulated Robot Path Non-Existence

72. No path exists! Roadmap 9 secs Path Search 0.18 secs

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 Complete methods PRM methods Our method Completeness Simplicity Probabilistically complete Cannot handle path non-existence Complete provided star-shaped sampling condition is met Handles path non-existence

78. Comparison PRM MethodsStar-shaped Roadmap Method Difficult to implement for high dofEasily extends to very high dofs Requires local planningNo explicit local planning

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 DecompStar-shaped Roadmap Method Resolution-completeComplete, 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 PRMStar-shaped Roadmap Method Objective is to generate a probabilistic roadmap with fewer nodes Objective is to do complete planning Randomized samplingDeterministic sampling Computes inter-sample visibility Star-shaped property defines the visibility of an entire region [Simeon et al. 2000]

82. Main Results 1.Star-shaped roadmaps for complete motion planning –Provides rigorous guarantees

83. Main Results 1.Star-shaped roadmaps for complete motion planning –Provides rigorous guarantees 2.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 –3T+3R 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 – 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

89. Connector Computation Connector –A point that connects the free space of two adjacent regions R i and R j if they are connected. –It lies on the shared boundary R ij and belongs to F RiRi RjRj R ij

90. Connector Computation Compute a subdivision of R ij into regions Q satisfying the star-shaped condition in one lower dimension, i.e. F Q = 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 R i and R j 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 R a1a1 b1b1 b2b2 a2a2 a0a0 n (a 0 - a 1 ). n >= 0 (a 1 - b 1 ). n <= 0 (a 2 - a 1 ). n >= 0 ^ => O

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. AB C

99. F

100. Star-Shaped Test: Boolean Combination 1.If the primitives are linear –Combine the linear constraints of all the primitives –Use linear programming 2.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 3.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 DecompStar-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 cellsNot 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] local path

105. Main Results Current work focused on robots with low degrees of freedom (dofs): –2T+1R 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 1.“Narrow passage” problem –Planner may not find a path even if a valid path exists –Especially through narrow passages

107. Issues 1.“Narrow passage” problem –Planner may not find a path even if a valid path exists –Especially through narrow passages 2.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 F R = 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 Complete methods PRM methods Our method Completeness Simplicity Efficiency Probabilistically complete Cannot handle path non-existence Complete provided star-shaped sampling condition is met Handles path non-existence

116. 2T+1R: Gears Configuration Space Contact Surfaces Goal Start Path

117. Assembly Configuration Space Contact Surfaces GoalStart Path

118. Candidate Point Estimation 1. Compute samples on the nonlinear primitive. 4.If a feasible point exists, use it as a candidate point [Varadhan et al. 2004] 2. Each sample defines a linear constraint. 3.Do linear programming

119. Verification A surface S is star-shaped w.r.t a point p if n (x – p) > 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) > 0  x  S where n is the normal at point x

121. Configuration Space Approximation x y  12 secs 1,550 contact surfaces Free Space Approximation Maze Example

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. 2T+1R: Gears Free Configuration Space Approximation Goal Start Path x y 

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]