Lecture 4: Improving the Quality of Motion Paths

1. Lecture 4: Improving the Quality of Motion Paths Software Workshop:High-Quality Motion Paths for Robots (and Other Creatures) TAs: Barak Raveh, barak@post.tau.ac.il and Naama Mayer, naamamay@post.tau.ac.il School of Computer Science, Tel-Aviv University

2. source ? target Reminder: The Motion Planning Problem Planning the motion of a k-dimensional robot (or a moving object) among obstacles The world Workspace with(static or moving) obstacles Complexity: NP-hard with respect to number of robot degrees of freedom (Canny and Reif, 87’) Robot configuration Defined by k degrees of freedom Motion Query From source configuration to target configuration

3. High-quality Paths Short paths / “high-clearance” paths (away from obstacles) / smooth paths / low-energy paths (in physical systems): NP-complete even in very simple settings(e.g., Canny and Reif, 87’)

4. Path Quality: Some Analytical Solutions for Translation in 2D http://www.sfbtr8.uni-bremen.de/project/r3/HGVG/hierarchicalVGraphs.html High clearance: the Generalized-Voronoi Diagram (GVD) Mixed: the Visibility-Voronoi Diagram (Wein et al., 2007) See also in: http://cse.stanford.edu/class/sophomore-college/projects-98/robotics/basicmotion.html

5. Reminder: Sampling-based “Roadmap” Algorithms for High-Dimensional Motion Planning • Probabilistic Roadmap (PRM, Kavraki et al., 96’) • Rapidly-exploring Random Trees (RRT, LaValle and Kuffner, 01’) • Expansive-Space Trees (EST, Hsu et al. 99’) • SBL[SBL, Sànches and Latombe, 02’] • PRM Algorithm – example in two-dimensional configuration space: • Randomly sample n valid robot configurations • Connect close-by configurations by dense sampling (“local-planning”) • Discard invalid edges

6. Some Relevant Ideas for Improving Path Quality in Sampling-Based Methods Path Length: • Self-shortcuts of output pathway: • Probabilistic road-maps with cyclesrather slow – road-map size increases quadratically • PRM with useful cycles – adding only significant short-cuts to road-map (Nieuwenhuisen et al., 04’) • Path Clearance: • Improving paths clearance by iteratively retracting into the medial-axis(Wilmarth et al. 97’, Geraerts et al. 07’)

7. PRM with Useful Cycles (for Finding Short Paths) (Nieuwenhuisen et al., Useful cycles in probabilitic roadmap graphs, 2004) • Recall our talk about connection strategies (lecture 2) • New connection strategy:add only K-useful edges to the roadmap • Definition of a K-useful edge between c and c’: K∙d(c,c ') < G(c,c ' )d(c,c’) = distance between c and c’G(c,c’) = graph distance between c and c’ d(c,c ') 1 c’ c 0.5 2 1.5 1

8. Some Sample Problems [Nieuwenhuisen et al., 04’]

9. Performance in Scene 1:Grid of Obstacles The shortest path goes through the middle of the grid Comparison to optimal shortest path query path smoothed path Running time (180 milestones): Useful Cycles: 0.80 seconds PRM w/o cycles: 0.45 seconds Smoothing: +0.20 seconds [Nieuwenhuisen et al., 04’]

10. Performance in Scene 2:Random Polygons In this case, smoothing solves the problem query path smoothed path Running time (250 milestones): Useful Cycles: 3.3 seconds PRM w/o cycles: 2.3 seconds [Nieuwenhuisen et al., 04’]

11. Performance in Scene 3:Save the Flamingo The challenge is to find the correct hole query path smoothed path Running time (150 milestones): Useful Cycles: 7.4 seconds PRM w/o cycles: 5.5 seconds Smoothing: 2 seconds [Nieuwenhuisen et al., 04’]

12. Performance in Scene 4:Getting Out of the House Long path goes through the garden, short path goes straight through the house query path smoothed path Running time (350 milestones): Useful Cycles: 11 seconds PRM w/o cycles: 9.5 seconds Smoothing: 1 seconds [Nieuwenhuisen et al., 04’]

13. Summary ofUseful Cycles • Aimed for finding short paths • Some price for additional running time, but significant improvement of results

14. Improving the Quality of Motion Paths with Hybrydization-Graphs Raveh, Enosh and Halperin, ICR 2008 Enosh, Raveh, Schueler-Furman, Halperin and Ben-Tal, Biophys J. 2008

15. 3-D Example: Move the Rod from Bottom to Top of a 2-D grid target: source:

16. Randomly Generated Motion Path

17. 3 Randomly Generated Motion Paths:

18. π1 π2 π3 2 1 1 1 2 1 1 1 1 1 1 1 1 1 0.2 1.2 1 1 1 1.5 1 1 1 H-Graphs: Hybridizing Multiple Motion Paths ( = looking for shortcuts) Generality: the original motion planning algorithm is treated as a black-box

19. π1 π2 π3 2 1 1 1 2 1 1 1 1 1 1 1 1 1 1.2 1 1 1 1.5 1 1 1 Hybridizing Three Random Motion Paths

20. Generality of Quality Criteria Quality Measure The Input Paths H-Graph Output Path Clearance and length (emphasis on clearance) Clearance and length (emphasis on length) Path length

21. Finding High Clearance Paths: Input

22. Finding High-Clearance Paths: Output

23. 12 Degrees of Freedom: switching between two wrenches among metal beams (rotation + translation, x2) H-Graphs for hybridizing six random path improved clearance from 0 clearance (touching the obstacle beams) to 20% of the wrench width

24. π1 π2 π3 Running-Time Bottleneck: Trying to Connect Nodes from Different Paths In a naïve implementation: O(n2) potential edges need to be tested Simple Heuristic – “Neighborhood H-Graphs”: compare only to nodes in local neighborhood – but can we do better?

25. Edit Distance String Matching Linear Alignments Comparing “This dog” and “That Dodge” with insertion / deletions / replacement: T H I – S D O – G – T H A T – D O D G E Classical dynamic-programming algorithm: insertion deletion replacement

26. π1 π2 π3 Alignment Length is LinearNow testing only O(n) edges along the alignment

27. Comparison of Running Times • Hybridizing 5 motion paths in a 2-D maze: • From 3.52 seconds to 0.83 seconds on average, with similar path quality

28. Phase I Phase II Phase III (Enosh, Raveh et al., 2008) Low-Energy Molecular Motions with 104 Degrees of Freedom Generating + hybridizing 20 simulated RRT motion paths with 104 DOFs: Molecular Energy Along Motion Path Energy Score Trajectory Step

29. Path Alignment in Molecular Example Path alignment saves expensive energy calculation time Path P Path Q

30. Summary ofHybridization Graphs • Generality with respect to: • Motion planning algorithm of input • Path optimality criteria • Edit-distance H-graphs • saving expensive calculation time by alignment of input motion path (quadratic  linear) • The price – producing more than one random path