Transcript Hybridization-Graphs for Improving Path Quality in

Lecture 4: Improving the Quality of
Motion Paths
Software Workshop:
High-Quality Motion Paths for Robots (and Other Creatures)
TAs: Barak Raveh, [email protected] and Naama Mayer, [email protected]
School of Computer Science, Tel-Aviv University
Reminder: The Motion
Planning Problem
Planning the motion of a k-dimensional robot (or a
moving object) among obstacles
Complexity: NP-hard with
respect to number of robot
degrees of freedom
source
(Canny and Reif, 87’)
target
The world
Workspace with
(static or moving)
obstacles
Robot
configuration
Defined by k degrees of
freedom
Motion Query
From source
configuration to target
configuration
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
http://www.sfbtr8.unibremen.de/project/r3/HGVG/hierarchicalVGraphs.html
Shortest path: the
Visibility graph
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
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 twodimensional 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’
c
1
0.5
2
1.5
1
Some Sample Problems
[Nieuwenhuisen et al., 04’]
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’]
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’]
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’]
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’]
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
target:
source:
Randomly Generated Motion Path
3 Randomly Generated Motion Paths:
H-Graphs: Hybridizing Multiple
Motion Paths ( = looking for shortcuts)
π1
1
π2
2
1
Generality: the original
1
motion planning
algorithm is treated
as
1
a black-box 1 1.2
1
π3
1
2
1
1
1
1
1
1
0.2
1
1
1.5
1
1
1
Hybridizing Three Random
Motion Paths
π1 π2 π3
1
2
1 2 1
1
1
1
1
1
1
1
1
1
1
1
1.
2
1
1
1.5
1
1
Generality of
Quality Criteria
Quality
Measure
Clearance
and length
(emphasis on
clearance)
Clearance and
length
(emphasis on
length)
Path length
The Input
Paths
H-Graph
Output Path
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
In a naïve implementation:
O(n2) potential edges need
to be tested
π1
π2
π3
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:
THI – S DO–G–
THAT– DODGE
Classical dynamic-programming algorithm:
insertion
deletion
replacement
Alignment Length is Linear
Now testing only O(n) edges along the alignment
π1
π2 π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
Low-Energy Molecular Motions
with 104 Degrees of Freedom
(Enosh, Raveh et al., 2008)
Generating + hybridizing 20 simulated
RRT motion paths with 104 DOFs:
Molecular Energy Along Motion Path
Phase II
Energy Score
Phase I
Trajectory Step
Phase III
Path Alignment in Molecular Example
Path alignment saves expensive energy calculation time
Path P
Path Q
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