Lecture 4: Improving the Quality of Motion Paths Software Workshop: High-Quality Motion Paths for Robots (and Other Creatures) TAs: Barak Raveh, and Naama Mayer, School of Computer Science, Tel-Aviv University

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

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’)

Path Quality: Some Analytical Solutions for Translation in 2D Shortest path: the Visibility graph High clearance: the Generalized-Voronoi Diagram (GVD) Mixed: the Visibility-Voronoi Diagram (Wein et al., 2007) See also in: bremen.de/project/r3/HGVG/hierarchicalVGraphs.html

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

Some Relevant Ideas for Improving Path Quality in Sampling-Based Methods Path Length: Self-shortcuts of output pathway: Probabilistic road-maps with cycles rather 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’)

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’ c’ d(c,c ') c

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

Performance in Scene 1: Grid of Obstacles Running time (180 milestones): Useful Cycles: 0.80 seconds PRM w/o cycles: 0.45 seconds Smoothing: seconds query pathsmoothed path The shortest path goes through the middle of the grid Comparison to optimal shortest path

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

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

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

Summary of Useful Cycles Aimed for finding short paths Some price for additional running time, but significant improvement of results

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

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

Randomly Generated Motion Path

3 Randomly Generated Motion Paths:

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

Hybridizing Three Random Motion Paths π1π1 π2π2 π3π

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

Finding High Clearance Paths: Input

Finding High-Clearance Paths: Output

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

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

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

Alignment Length is Linear Now testing only O(n) edges along the alignment π1π1 π2π2 π3π3

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

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

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

Summary of Hybridization 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