Motion Planning & Robot Planning

1. Motion Planning & Robot Planning Prof.: S. Shiry Mohsen gandomkar M.Sc Artificial Intelligence Department of Computer Eng. and IT Amirkabir Univ. of Technology (Tehran Polytechnic)

2. What is Motion Planning? • Motion planning is aimed at providing robots with the capability of deciding automatically which motions to execute in order to achieve their tasks without colliding with other objects in their work space

3. Basic Definition • Obstacles • Already occupied spaces of the world • In other words, robots can’t go there • Free Space • Unoccupied space within the world • Robots “might” be able to go here • To determine where a robot can go, we need to discuss what a Configuration Space is

4. The Configuration Space Configuration Space of A is the space (C) of all possible configurations of A. C Cfree qgoal Cobs qstart For a point robot moving in 2-D plane, C-space is

5. The Configuration Space C y Cfree qgoal Z Cobs qstart x For a point robot moving in 3-D, the C-spaceis What is the difference between Euclidean space and C-space?

6. The Configuration Space • How to create it • First abstract the robot as a point object. Then, enlarge the obstacles to account for the robot’s footprint and degrees of freedom • In our example, the robot was circular, so we simply enlarged our obstacles by the robot’s radius (note the curved vertices)

7. Example of a World (and Robot) Free Space Obstacles Robot x,y

8. Configuration Space:Accommodate Robot Size Free Space Obstacles Robot (treat as point object) x,y

9. Motion Planning • Basic problem: Collision-free path planning for one rigid or articulated object (the “robot”) among static obstacles. • Inputs • geometric descriptions of the obstacles and the robot • kinematic and dynamic properties of the robot • initial and goal positions (configurations) of the robot • Output • Continuous sequence of collision-free configurations connecting the initial and goal configurations.

10. Algorithmic Approaches • Complete Algorithms • Probabilistic Algorithms • Heuristic Algorithms

11. Complete Algorithms • Guaranteed to find a free path between two give configurations when exists and report failure otherwise • Deal with connectivity of free space by capturing it on a graph. • Cell Decomposition - partition of free space • Roadmap Technique - network of curves

12. Probabilistic Algorithms • Trade-off exactness against running time • Don’t guarantee a solution but if exists very likely to find it relatively quickly • Example: Probabilistic Roadmap Algorithm • Experimental results show that computation takes less than a second

13. Heuristic Algorithms • Many work well in practice but offer no performance guarantee • Deal with a grid on configuration space • Example 1 : Potential Field • Example 2 : Approximate Cell Decomposition

14. Previous Approaches

15. Visibility Graphs

16. Voronoi Diagrams

17. Exact Cell Decomposition

18. Approximate Cell Decomposition

19. Potential Fields

20. Probabilistic Roadmaps Method

21. Problems before PRMs • Hard to plan for many dof robots • Computation complexity for high-dimensional configuration spaces would grow exponentially • Potential fields run into local minima • Complete, general purpose algorithms are at best exponential and have not been implemented

22. Difficulty with classic approaches • Running time increases exponentially with the dimension of the configuration space. • For a d-dimension grid with 10 grid points on each dimension, how many grid cells are there? • Several variants of the path planning problem have been proven to be PSPACE-hard. 10d

23. Probabilistic Roadmap (PRM): multiple queries local path milestone free space [Kavraki, Svetska, Latombe,Overmars, 96]

24. Probabilistic Roadmap (PRM): single query

25. Multiple-Query PRM

26. Classic multiple-query PRM • Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces, L. Kavraki et al., 1996.

27. Assumptions • Static obstacles • Many queries to be processed in the same environment • Examples • Navigation in static virtual environments • Robot manipulator arm in a workcell

28. Enter PRMs • PRMs use fast collision checking techniques • PRMs avoid computing an explicit representation of the configuration space • Two Phases • A Learning Phase • A Query Phase

29. The Learning Phase • Construct a probabilistic roadmap by generating random free configurations of the robot and connecting them using a simple, but very fast motion planer, also know as a local planner • Store as a graph whose nodes are the configurations and whose edges are the paths computed by the local planner

30. PRM - Learning Phase Free space C-obstacle

31. PRM - Learning Phase Free space C-obstacle

32. PRM - Learning Phase Free space C-obstacle milestones

33. PRM - Learning Phase Free space C-obstacle milestones

34. The Query Phase • Find a path from the start and goal configurations to two nodes of the roadmap • Search the graph to find a sequence of edges connecting those nodes in the roadmap • Concatenating the successive segments gives a feasible path for the robot

35. CLEAR(q)Is configuration q collision free or not? LINK(q, q’) Is the straight-line path between q and q’ collision-free? Two geometric primitives in configuration space

36. Uniform sampling 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: ifCLEAR(q)then 5:Add q to V. 6: Nq  a set of nodes in V that are close to q. 6: for each q’ Nq, in order of increasing d(q,q’) 7: ifLINK(q’,q)then 8: Add an edge between q and q’ to E.

37. Difficulty • Many small connected components

38. Resampling (expansion) • Failure rate • Weight • Resampling probability

39. Resampling (expansion)

40. Query processing • Connect qinit and qgoal to the roadmap • Start at qinit and qgoal, perform a random walk, and try to connect with one of the milestones nearby • Try multiple times

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

42. Why does it work? Intuition • A small number of milestones almost “cover” the entire configuration space.

43. Smoothing the path

44. Smoothing the path

45. Single-Query PRM

46. Lazy PRM • Path Planning Using Lazy PRM, R. Bohlin & L. Kavraki, 2000.

47. Precomputation: roadmap construction • Nodes • Randomly chosen configurations, which may or may not be collision-free • No call to CLEAR • Edges • an edge between two nodes if the corresponding configurations are close according to a suitable metric • no call to LINK

48. Query processing: overview • Find a shortest path in the roadmap • Check whether the nodes and edges in the path are collision. • If yes, then done. Otherwise, remove the nodes or edges in violation. Go to (1). • We either find a collision-free path, or exhaust all paths in the roadmap and declare failure.

49. Query processing: details • Find the shortest path in the roadmap • A* algorithm • Dijkstra’s algorithm • Check whether nodes and edges are collisions free • CLEAR(q) • LINK(q0, q1)

50. Node enhancement • Select nodes that close the boundary of F