Dynamic-Domain RRTs Anna Yershova, Steven M. LaValle 03/08/2006

Basic Motion Planning Problem Given:  2D or 3D world  Geometric models of obstacles  Geometric models  Configuration space  Initial and goal configurations Task:  Compute a collision free path that connects initial and goal configurations

Rapidly-exploring Random Trees (RRTs)  Introduced by LaValle and Kuffner, ICRA  Applied, adapted, and extended in many works: Frazzoli, Dahleh, Feron, 2000; Toussaint, Basar, Bullo, 2000; Vallejo, Jones, Amato, 2000; Strady, Laumond, 2000; Mayeux, Simeon, 2000; Karatas, Bullo, 2001; Li, Chang, 2001; Kuffner, Nishiwaki, Kagami, Inaba, Inoue, 2000, 2001; Williams, Kim, Hofbaur, How, Kennell, Loy, Ragno, Stedl, Walcott, 2001; Carpin, Pagello, 2002; Branicky, Curtiss, 2002; Cortes, Simeon, 2004; Urmson, Simmons, 2003; Yamane, Kuffner, Hodgins, 2004; Strandberg, 2004;...  Also, applications to biology, computational geography, verification, virtual prototyping, architecture, solar sailing, computer graphics,...

The RRT Construction Algorithm GENERATE_RRT(x init, K,  t) 1.T.init(x init ); 2.For k = 1 to K do 3. x rand  RANDOM_STATE(); 4. x near  NEAREST_NEIGHBOR(x rand, T); 5. if CONNECT(T, x rand, x near, x new ); 6. T.add_vertex(x new ); 7. T.add_edge(x near, x new, u); 8.Return T; x near x init x new The result is a tree rooted at x init

A Rapidly-exploring Random Tree (RRT)

Voronoi Biased Exploration Is this always a good idea?

Voronoi Diagram in R 2

Refinement vs. Expansion refinementexpansion Where will the random sample fall? How to control the behavior of RRT?

Limit Case: Pure Expansion  Let X be an n -dimensonal ball, in which r is very large.  The RRT will explore n  1 opposite directions.  The principle directions are vertices of a regular  n  1- simplex

Determining the Boundary Expansion dominatesBalanced refinement and expansion The tradeoff depends on the size of the bounding box

Controlling the Voronoi Bias  Refinement is good when multiresolution search is needed  Expansion is good when the tree can grow and not blocked by obstacles Main motivation:  Voronoi bias does not take into account obstacles  How to incorporate the obstacles into Voronoi bias?

Bug Trap Which one will perform better? Small Bounding Box Large Bounding Box

Voronoi Bias for the Original RRT

Visibility - Based Clipping of the Voronoi Regions Nice idea, but how can this be done in practice? Even better: Voronoi diagram for obstacle-based metric

(a) Regular RRT, unbounded Voronoi region (b) Visibility region (c) Dynamic domain A Boundary Node

A Non-Boundary Node (a) Regular RRT, unbounded Voronoi region (b) Visibility region (c) Dynamic domain

Dynamic-Domain RRT Bias

Dynamic-Domain RRT Construction

Dynamic-Domain RRT Bias Tradeoff between nearest neighbor calls and collision detection calls

Recent Efforts Adaptive tuning of the radius:  the radius is not fixed but is increased with every extension success and is decreased with every failure Nearest neighbor calls:  kd-tree based implementation  O(log n) instead of naïve O(n) query time Uniform sampling from dynamic domain:  Rejection-based method is not efficient for high dimensions  Uniform distribution should be generated directly

Adaptive Tuning of Parameter

Nearest Neighbor Calls & Uniform Sampling  Efficient implementation using kd-trees  O(log n) query time instead of naïve O(n) query time

The kd-tree is a powerful data structure that is based on recursively subdividing a set of points with alternating axis-aligned hyperplanes. The classical kd-tree uses O(dn lgn) precomputation time, O(dn) space and answers queries in time logarithmic in n, but exponential in d l5l5 l1l1 l9l9 l6l6 l3l3 l 10 l7l7 l4l4 l8l8 l2l2 l1l1 l8l8 1 l2l2 l3l3 l4l4 l5l5 l7l7 l6l6 l9l KD-trees

Kd-trees. Construction l5l5 l1l1 l9l9 l6l6 l3l3 l 10 l7l7 l4l4 l8l8 l2l2 l1l1 l8l8 1 l2l2 l3l3 l4l4 l5l5 l7l7 l6l6 l9l

Kd-trees. Query l5l5 l1l1 l9l9 l6l6 l3l3 l 10 l7l7 l4l4 l8l8 l2l2 l1l1 l8l8 1 l2l2 l3l3 l4l4 l5l5 l7l7 l6l6 l9l q

Shrinking Bug Trap Large Medium Small

The smaller the bug trap, the better the improvement Shrinking Bug Trap

Wiper Motor (courtesy of KINEO)  6 dof problem  CD calls are expensive

Molecule  68 dof problem was solved in 2 minutes  330 dof in 1 hour  6 dof in 1 min. 30 times improvement comparing to RRT  CD calls are expensive

Labyrinth  3 dof problem  CD calls are not expensive

3D grid  6 dof problem  CD calls are not expensive

Spiral  6 dof problem  CD calls are not expensive

Conclusions  Controlling Voronoi bias is important in RRTs.  Provides dramatic performance improvements on some problems.  Does not incur much penalty for unsuitable problems. Work in Progress:  Application to planning under differential constraints.  Application to planning for closed chains.