1. Randomized Approaches to Motion-Planning Problem Hennadiy Leontyev The University of North Carolina at Chapel Hill

2. Talk Outline Brief review of path-planning. Rapidly-growing Random Trees. Randomized Roadmaps. Ariadne’s Clew algorithm. Conclusion.

3. Configuration Space Model Path planning is usually performed in configuration space C. Path is a subset of C free. Explicit representation of free space C free is not usually available. We can test whether certain configuration q is in C free. Real World Start Configuration Space is a bizarre 3D manifold R 2 xS! ? Finish

4. Aspects of Path-Planning Dynamic or static? Single-query or multiple-query? How fast should it be? Randomized or complete?

5. Talk Outline Brief review of path-planning. Rapidly-growing Random Trees [1]. Randomized Roadmaps [2]. Ariadne’s Clew algorithm [3]. Conclusion.

6. RRT-Basic ( the algorithm ) BUILD-RRT constructs a tree of sample points in C free, which is rooted at initial configuration. The procedure RandomConfig returns a random point from C free The procedure EXDEND spans the tree as much as possible to given direction. The procedure NEW-CONFIG is a local movement planning function. BUILD-RRT(q init ) T.init(q init ) for i=1 to K do q next =RandomConfig() EXTEND(T,q next ) return T. EXTEND(T,q) q near =NEAREST-NEIGHBOR(T,q) If NEW-CONFIG(q,q near,q new ) Then T.addVertex(q new ) T.addEdge(q near,q new ) If q new =q Then return Reached Else return Advanced

7. RRT-Basic ( in action ) q init An obstacle q near q q new Configurations in the tree Goal configuration e EXTEND advances to the distance at most e

8. RRT-Connect ( the algorithm ) Extends RRT-basic. Uses two trees T i and T g to find the path. Aggressive tree expansion. CONNECT(T,q) repeat S=EXTEND(T,q) until S!=Advanced RRT-CONNECT(q init,q goal ) T i.init(q init ); T g.init(q goal ) for j:=1 to K do q rand :=RANDOM-CONFIG() If EXTEND(T i, q rand )!=Trapped Then If CONNECT(T g,q new )=Reached Then return PATH(T i,T g ) SWAP(T i,T g ) return Failure

9. RRT-Connect ( in action ) q init q new q goal q rand TiTi Configurations in the tree T i Goal configuration Initial configuration Configurations in the tree T g TgTg

10. RRT-Connect ( performance ) In RRT-Basic search is biased by large Voronoi regions. The tree covers unexplored space rapidly. Two search trees of RRT-Connect are growing towards each other

11. RRT-Connect ( performance ) 270 MHz Silicon O2 workstation. ~1 sec. to find a solution for examples on the right. Grand Piano moving problem (4500 triangles) was solved in 12 sec. Virtual chess player arm modelled as 7-DOF kinematic chain and 8000 triangles required 2 sec to move a figure.

12. RRT-Connect ( conclusion ) Does not require parameter tuning. Preprocessing is not required. Simple and consistent behavior is observed. The method is well-suited for incremental distance computation and nearest-neighbor algorithms.

13. Talk Outline Brief review of path-planning. Rapidly-growing Random Trees [1]. Randomized Roadmaps [2]. Ariadne’s Clew algorithm [3]. Conclusion.

14. Randomized Roadmaps ( the algorithm ) Planning is also performed in C-space. Sample points are generated on obstacle surfaces δC free (collision detection algorithms are heavily used). These sample points are then connected to a roadmap. S={s i }-obstacles in real-world S’={s i ’}-obstacles in C-space V i – candidate nodes V - final set of nodes For each object s i ’ in S’ do V i :=GenerateRandomPoints(s i ’) V:=V+V i For each p in V do For each s j ’ in S’ do If s j ’.contains(p) Then V:=V-p BUILD-ROADMAP(V)

15. Randomized Roadmaps ( sampling) Find an origin point o inside the object. Generate m uniformly distributed directions. Use binary search to identify boundaries. + Rather simple implementation. - Not good for “thin” objects. Candidate points Object boundaries o

16. Randomized Roadmaps ( improvements) How do we select a center? How do we deal with “thin” objects? What happens if the object has folds?

17. Randomized Roadmaps ( selecting object center) o d2d2 d1d1 Random origin is bad: the distance between sample points is uneven. Center point Object boundaries Using the center of masses as an origin within the object o d2d2 d1d1 Bad news: Needs more time for preprocessing and fast algorithms.

18. Randomized Roadmaps ( dealing with “thin” objects) For “thin” objects no selection of origin will give good distribution. o d2d2 d1d1 Sample points Object boundaries Origin Bad news: Requires knowledge of object topology and more preprocessing time. o d2d2 d1d1 Auxiliary points Use auxiliary points to cover spaces.

19. Randomized Roadmaps ( dealing with foldings) If we know that the object “folds” this does not help to avoid complex computations. Sample points Object boundaries Origin o

20. Randomized Roadmaps ( building the roadmap ) Connect samples from different objects using a local planner. –How accurate the map should be? –What resources do we have to build the map? –This affects the number of connections (usually a tradeoff). Identify connected components. Build “corridors” that determine collision-free paths in C-space Boundary points Collision-free route

21. Randomized Roadmaps ( planning) Connect start and finish nodes with a roadmap by simple or random planner Identify the path between roadmap points Build the full path. Boundary points Collision-free route q 0 – start configuration q goal – goal

22. Randomized Roadmaps ( performance) 100 MHz MIPS R4600. 6-DOF articulated robot with fixed base. E 1 – five objectsE 2-5 – two objects with a narrow passage Time to connect configurations is near 1/100 of a second.

23. Randomized Roadmaps ( conclusion ) Multiple planning queries could be served quickly. The preprocessing could be parallelized. The complexity of the map depends on the number of obstacles (may be huge for cluttered environments). Significant amount of preprocessing is required. Taking good sample points is a non-trivial task.

24. Talk Outline Short definitions of path-planning. Rapidly-growing Random Trees [1]. Randomized Roadmaps [2]. Ariadne’s Clew algorithm [3]. Conclusion.

25. Ariadne’s Clew ( the algorithm ) Search is performed in trajectory space (a 1,r 1,a 2,r 2,…a l,r l ) which leads to q l – this path has k*l steps in it, where k is the number of DOF. Exists a distance function d(q l,q goal ). The combination of two routines: SEARCH and EXPLORE SEARCH attempts to find a configuration that minimizes the distance to the goal. EXPLORE attempts to approximate free space by a set of landmarks to which the path of l steps is known. a1a1 a2a2 a3a3 alal r1r1 r2r2 r3r3 qlql q0q0 q0q0 Initial configuration qlql Final configuration aiai Rotation angle riri Distance in the world

26. Ariadne’s Clew ( search ) qjqj q goal q0q0 min l d(q l,q goal ) Goal configuration Min distance configuration Non-mins +Tries to find a configuration with minimal distance to the goal. -- Could be trapped in local minima -- Length of trajectories may be too small Attempts to find a path of l steps length that minimizes distance min R l d(q l,q goal ) to the goal. qkqk qjqj qlql

27. Ariadne’s Clew (explore) Path from initial position to any landmark is known. Landmarks are spread uniformly in C. The procedure finds a configuration which maximizes the distance and path no longer than l. + Approximates free space by a set of landmarks --Knows nothing about the goal max l d(q l,L) New landmark Previously set landmarks L qlql

28. Ariadne’s Clew ( in action ) ALGORITHM-ARIADNE(q 0,q goal,r) i:=1; λ i =q 0 L i ={λ i };e=infinity while(e>r) /* Run local planner SEARCH */ if( min l d(q goal,q l )=0) Then return Path else /* Place new landmark with EXPLORE*/ i:=i+1 λ i :=q:sup l d(L i-1,q l ) L i :=L i-1 + {λ i } e=d(L i-1, λ i ) L=L i return NO PATH Landmark (given by EXPLORE) Config found by SEARCH Initial config Goal configuration

29. Ariadne’s Clew ( performance ) Two 6-DOF arms are used. One arm is a robot, the other performs random movements. Meganode of 128 T800 (25MHz) transputers is used to make planning. Parallel genetic algorithm implementation. –Trajectories are Manhattan paths of order 2. R 12 for SEARCH and (i,R 12 ) for EXPLORE. –Each DOF is discretized with 9 bits. –Max number of landmarks 256. –All possible data is encoded with 119 bit strings.

30. 1.Generate a random movement for Robot B 2.Send the position of B and desired position of A to the Meganode. 3.Get planned path for A from the Meganode and execute it. 4.Wait for a random time and stop A, goto 1. The mean time to compute a single path for the new environment is 1.5 sec.

31. Ariadne’s Clew ( conclusion ) SEARCH and EXPLORE could work in parallel over the same set of landmarks. Thus the performance of the algorithm is improved greatly by using parallel search and collision-detection. Constraints might not be only obstacles but rather arbitrary constraints on paths. Experiments showed that the algorithm performs well in realistic dynamic environments.

32. Conclusion RRTRandomized Roadmaps Ariadne’s Clew Number of queries SingleMultipleSingle PreprocessingNoYesNo Is dynamicBarelyNoYes Implementation complexity LowHighMedium ParallelizableNoYes

33. References [1] Kuffner J.J. Jr. and LaValle S.M. “RRT-connect: An efficient approach to single-query path planning.” In Proceedings of the ’00. IEEE International Conference on Robotics and Automation, vol. 2 pages 995-1001, April 2000. [2] Amato N.M and Wu Y. “A randomized roadmap method for path and manipulation planning.”, In Proceedings of the 1996 International Conference on Robotics and Automation, vol. 1, pages 113—120, April 1996. [3] Jean Manuel Ahuactzin, Emmanuel Mazer and Pierre Bessiere “The ariadne’s clew algorithm.”, Journal of Artificial Intelligence Research, 9, 1998. Steven LaValle. “Planning Algorithms.”, Cambridge University Press, 2006.