1 Single Robot Motion Planning - II Liang-Jun Zhang COMP Sep 24, 2008

2 Review: C-space Workspace Configuration Space x y Robot Initial Goal Free Obstacle C-obstacle A 2D Translating Robot

3 Review: Computing C-obstacle Difficult due to geometric and space complexity Practical solutions are only available for –Translating rigid robots: Minkowski sum –Robots with no more than 3 DOFs

4 Outline Approximate cell decomposition Sampling-based motion planning

5 Approximate Cell Decomposition (ACD) Not compute the free space exactly at once But compute it incrementally Relatively easy to implement –[Lozano-Pérez 83] –[Zhu et al. 91] –[Latombe 91] –[Zhang et al. 06] Octree decomposition

6 fullmixed empty Approximate Cell Decomposition Full cell Empty cell Mixed cell –Mixed –Uncertain Cell labelling algorithms –[Zhang et al 06] Configuration Space

7 Finding a Path by ACD Goal Initial

8 Connectivity Graph G f : Free Connectivity GraphG: Connectivity Graph G f is a subgraph of G

9 Finding a Path by ACD Goal Initial G f : Free Connectivity Graph

10 Finding a Path by ACD L: Guiding Path First Graph Cut Algorithm –Guiding path in connectivity graph G –Only subdivide along this path –Update the graphs G and G f

11 First Graph Cut Algorithm Only subdivide the cells along L L : Guiding Path new G f

12 Finding a Path by ACD GfGf A channel Can be used for path smoothing.

13 ACD for Path Non-existence C-space Goal Initial

14 Connectivity Graph Guiding Path ACD for Path Non-existence

15 ACD for Path Non-existence Connectivity graph is not connected No path! A sufficient condition for deciding path non-existence

16 Live Demo –Gear-2DOF –Gear-3DOF

17 Five-gear Example Vide o Initial Goal roadmap in free space Total timing85s # of total cells168K

18 Two-gear Example no path! Cells in C-obstacle Initial Goal Roadmap in F Vide o 3.356s

19 Motion Planning Framework Continuous representation Discretization Graph search

20 Summary: Approximate Cell Decomposition Simple and easy to implement Efficient and practical for low DOF robots –Inefficient for 5 or more DOFs robot Resolution-complete –Find a path if there is one –Otherwise, report path non-existence –Up to some resolution of the cell

21 Outline Approximate cell decomposition Sampling-based motion planning

22 Motivation Geometric complexity Space dimensionality

23 Probabilistic Roadmap (PRM) free space q init q goal milestone [Kavraki, Svetska, Latombe,Overmars, 95] local path

24 Basic PRM Aalgorithm Input: geometry of the moving object & obstacles Output: roadmap G = (V, E) 1: V   and E  . 2: repeat 3: q  a configuration sampled uniformly at random from C. 4: if CLEAR(q)then 5: Add q to V. 6: N q  a set of nodes in V that are close to q. 6: for each q’  N q, in order of increasing d(q,q’) 7: if LINK(q’,q)then 8: Add an edge between q and q’ to E.

25 Two Geometric Primitives in C-space C LEAR( q ) Is configuration q collision free or not? L INK( q, q’) Is the straight-line path between q and q’ collision-free ?

26 Query Processing Connect q init and q goal to the roadmap Start at q init and q goal, perform a random walk, and try to connect with one of the milestones nearby Try multiple times

27 Two Tenets of PRM Planning  Checking sampled configurations and connections between samples for collision can be done efficiently.  Hierarchical collision checking [Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]  A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space.  Exponential convergence in expansive free space (probabilistic completeness)

28 Why does it work? Intuition A small number of milestones almost “cover” the entire free space.

29 Two Tenets of PRM Planning  Checking sampled configurations and connections between samples for collision can be done efficiently.  Hierarchical collision checking [Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]  A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space.  Exponential convergence in expansive free space (probabilistic completeness)

30 Narrow Passage Problem Narrow passages are difficult to be sampled due to their small volumes in C-space Narrow passage Alpha puzzle q init q goal Configuration Space

31 Difficulty Many small connected components

32 Strategies to Improve PRM Where to sample new milestones? –Sampling strategy Which milestones to connect? –Connection strategy Goal: –Minimize roadmap size to correctly answer motion-planning queries

33 Sampling Strategies

34 small visibility sets good visibility poor visibility Poor Visibility in Narrow Passages Non-uniform sampling strategies

35 But how to identify poor visibility regions? What is the source of information? –Robot and environment geometry How to exploit it? –Workspace-guided strategies –Dilation-based strategies –Filtering strategies –Adaptive strategies

36  Identify narrow passages in the workspace and map them into the configuration space Workspace-guided strategies

37 F O Dilation-based strategies During roadmap construction, allow milestones with small penetration Dilate the free space –[Hsu et al. 98, Saha et al. 05, Cheng et al. 06, Zhang et al. 07] A milestone with small penetration

38 Error If a path is returned, the answer is always correct. If no path is found, the answer may or may not be correct. We hope it is correct with high probability.

39 Weaker Completeness  Complete planner  Too slow  Heuristic planner  Too unreliable  Probabilistic completeness: If a solution path exists, then the probability that the planner will find one is a fast growing function that goes to 1 as the running time increases

40 Kinodynamic Planning

41 RRT for Kinodynamic Systems Rapidly-exploring Random Tree Randomly select a control input

42 More Examples Car pulling trailers (complicated kinematics -- no dynamics)

43 Summary: Sampling-based Motion Planning + Efficient in practice + Work for robots with many DOF (high- dimensional configuration spaces) + Has been applied for various motion planning problems (non-holonomic, kinodynamic planning etc.) -Narrow passages problems (one of the hot areas) -May not terminate when no path exists

44 Summary Configuration space Visibility graph Approximate cell decomposition –Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Sampling-based approach –High-dimensional Configuration Spaces –Capture the connectivity of the free space by sampling

45 References Books –J.C. Latombe. Robot Motion Planning, –S.M. LaValle, Planning Algorithms, 2006 Free book: –H. Choset et al. Principles of Robot Motion: Theory, Algorithms, and Implementations, 2005 Conferences –ICRA: IEEE International Conference on Robotics and Automation –IROS: IEEE/RSJ International Conference on Intelligent RObots and Systems –WAFR: Workshop on the Algorithmic Foundations of Robotics