Randomized Approaches to Motion-Planning Problem

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 Real World • Path planning is usually performed in configuration spaceC. • Path is a subset ofCfree. • Explicit representation of free space Cfree is not usually available. • We can test whether certain configuration q is in Cfree. Start Finish Configuration Space is a bizarre 3D manifold R2xS! ?

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. BUILD-RRT constructs a tree of sample points in Cfree, which is rooted at initial configuration. The procedureRandomConfigreturns a random point from Cfree The procedure EXDEND spans the tree as much as possible to given direction. The procedure NEW-CONFIG is a local movement planning function. RRT-Basic (the algorithm) BUILD-RRT(qinit) T.init(qinit) fori=1 to K do qnext=RandomConfig() EXTEND(T,qnext) return T. EXTEND(T,q) qnear=NEAREST-NEIGHBOR(T,q) If NEW-CONFIG(q,qnear,qnew) Then T.addVertex(qnew) T.addEdge(qnear,qnew) Ifqnew=qThenreturn Reached Elsereturn Advanced

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

8. RRT-Connect (the algorithm) CONNECT(T,q) repeat S=EXTEND(T,q) untilS!=Advanced • Extends RRT-basic. • Uses two trees Tiand Tgto find the path. • Aggressive tree expansion. RRT-CONNECT(qinit,qgoal) Ti.init(qinit); Tg.init(qgoal) forj:=1 to K do qrand:=RANDOM-CONFIG() If EXTEND(Ti, qrand)!=Trapped Then If CONNECT(Tg,qnew)=Reached Then return PATH(Ti,Tg) SWAP(Ti,Tg) return Failure

9. qrand qnew Goal configuration Configurations in the tree Tg Configurations in the tree Ti Initialconfiguration RRT-Connect (in action) Tg qinit qgoal Ti

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

11. 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. RRT-Connect (performance)

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. Planning is also performed in C-space. Sample points are generated on obstacle surfaces δCfree(collision detection algorithms are heavily used) . These sample points are then connected to a roadmap. Randomized Roadmaps (the algorithm) S={si}-obstacles in real-world S’={si’}-obstacles in C-space Vi – candidate nodes V - final set of nodes For each objectsi’inS’do Vi:=GenerateRandomPoints(si’) V:=V+Vi For each pinVdo For eachsj’ inS’ do Ifsj’.contains(p) Then V:=V-p BUILD-ROADMAP(V)

15. 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 Randomized Roadmaps (sampling) o

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

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

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

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

20. 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 Randomized Roadmaps (building the roadmap) Boundary points Collision-free route

21. q0– start configuration 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 qgoal– goal

22. 100 MHzMIPS R4600. 6-DOFarticulated robot with fixed base. Randomized Roadmaps (performance) E1 – five objects E2-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. Search is performed in trajectory space (a1,r1,a2,r2,…al,rl) which leads to ql – this path hask*lsteps in it, where k is the number of DOF. Exists a distance function d(ql,qgoal) . The combination of two routines: SEARCH and EXPLORE SEARCHattempts to find a configuration that minimizes the distance to the goal. EXPLOREattempts to approximate free space by a set of landmarks to which the path of l steps is known. Ariadne’s Clew (the algorithm) q0 a1 r1 a2 r2 r3 a3 al q0 Initial configuration ql ql Final configuration ai Rotation angle ri Distance in the world

26. qk ql qj qj Goal configuration Non-mins Min distance configuration minl d(ql,qgoal) Ariadne’s Clew (search) Attempts to find a path of lsteps length that minimizes distance minRl d(ql,qgoal) to the goal. q0 qgoal +Tries to find a configuration with minimal distance to the goal. -- Could be trapped in local minima -- Length of trajectories may be too small

27. 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 ql New landmark Previously set landmarksL maxl d(ql,L) Ariadne’s Clew (explore)

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

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

30. Generate a random movement for Robot B • Send the position of B and desired position of A to the Meganode. • Get planned path for A from the Meganode and execute it. • 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 andEXPLORE 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

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 ManuelAhuactzin, EmmanuelMazer and Pierre Bessiere “The ariadne’s clew algorithm.”, Journal of Artificial Intelligence Research, 9, 1998. • Steven LaValle. “Planning Algorithms.”, Cambridge University Press, 2006.