Exact Collision Checking of Robot Paths Fabian Schwarzer Mitul Saha Jean-Claude Latombe Computer Science Department Stanford University

Motivation (1) One tenet of PRM planning is that sampled configurations and connections can be efficiently tested for collision.

[Quinlan, 94; Gottschalk, Lin, Manocha, 96] Static collision tests (for sampled configurations) are done efficiently using pre-computed bounding volumes (BV) hierarchies Motivation (2)

But dynamic collision tests (for connections) using BV hierarchies are usually approximate. Motivation (3)    too large  collisions are missed   too small  slow test of local paths

Motivation (3)  But dynamic collision tests (for connections) using BV hierarchies are usually approximate.

Previous Approaches to Dynamic Collision Testing Bounding-volume (BV) hierarchies  Discretization issue Feature-tracking methods [Lin, Canny, 91] [Mirtich, 98] V-Clip [Cohen, Lin, Manocha, Ponamgi, 95] I-Collide [Basch, Guibas, Hershberger, 97] KDS  Geometric complexity issue with highly non-convex objects Swept-volume intersection [Cameron, 85] [Foisy, Hayward, 93]  Swept-volumes are expensive to compute. Too much data.

Our Approach Problem: Given two configurations, test if the straight path between them is collision-free, or not. Ideas: a)Relate configuration changes to path lengths in workspace b)Use distance computation rather than pure collision checking

For any q and q’ no robot point traces a path longer than: (q,q’) = 3|  q 1 |+2|  q 2 |+|  q 3 | q = (q 1,q 2,q 3 ) q’ = (q’ 1,q’ 2,q’ 3 )  q i = q’ i -q i q1q1 q2q2 q3q3 Ideas: a) Relate configuration changes to path lengths in workspace b) Use distance computation rather than pure collision checking

If (q,q’) <  (q) +  (q’) then the straight path between q and q’ is collision-free  (q)  (q) = Euclidean distance between robot and obstacles (or lower bound) q1q1 q2q2 q3q3 Ideas: a) Relate configuration changes to path lengths in workspace b) Use distance computation rather than pure collision checking

q q’ {q” | (q,q”) <  (q)} {q” | (q’,q”) <  (q’)} (q,q’) <  (q) +  (q’) Ideas: a) Relate configuration changes to path lengths in workspace b) Use distance computation rather than pure collision checking

q q’ {q” | (q,q”) <  (q)} {q” | (q’,q”) <  (q’)} Bisection (q,q’) >  (q) +  (q’) Ideas: a) Relate configuration changes to path lengths in workspace b) Use distance computation rather than pure collision checking

 Use BV hierarchy + same recursion as for pure collision checking  But compute distance between BVs instead of testing BV overlap   BVs are RSSs Greedy Distance Computation  returns lower bounds on distance that are often much larger than ½ actual distances  small factor slower than a pure collision checking  much faster than BV-based exact or approximate distance computation

Generalization Robot(s) and static obstacles treated as collection of rigid bodies A1, …, An. i (q,q’): upper bound on length of curve segment traced by any point on Ai when robot system is linearly interpolated between q and q’ 1 (q,q’) = |  q 1 | 2 (q,q’) = 2|  q 1 |+|  q 2 | 3 (q,q’) = 3|  q 1 |+2|  q 2 |+|  q 3 | q1q1 q2q2 q3q3

Generalization Robot(s) and static obstacles treated as collection of rigid bodies A1, …, An. i (q,q’): upper bound on length of curve segment traced by any point on Ai when robot system is linearly interpolated between q and q’ If i (q,q’) + j (q,q’) <  ij (q) +  ij (q’) then Ai and Aj do not collide between q and q’

Generalized Bisection Method I.Until Q is not empty do: 1.[q a,q b ] ij  remove-first(Q) 2.If i (q a,q b ) + j (q a,q b )   ij (q a ) +  ij (q b ) then a.q mid  (q a +q b )/2 b.If  ij (q mid ) = 0 then return collision c.Else insert [q a,q mid ] ij and [q mid,q b ] ij into Q II.Return no collision Each pair of bodies is checked independently of the others  priority queue Q of elements [q a,q b ] ij Initially, Q consists of [q,q’] ij for all pairs of bodies Ai and Aj that need to be tested.

Heuristic Ordering Q Goal: Discover collision quicker if there is one. Sort Q by decreasing values of: [ i (q a,q b ) + j (q a,q b )] – [  ij (q a ) +  ij (q b )] Possible extension to multi-segment paths (very useful with lazy collision-checking PRM)

Segment Covering Strategies Allows caching of forward kinematic results

Two Arms and Three Rings Two 20-dof linkages, with 320 triangles each Three rings with 6,300 triangles each

Robot in a Cage Robot: 2,991 triangles Cage: 432 triangles

Spot Welding Robot: 2,991 triangles Obstacles: 74,681 triangles

Comparative Experiment Robot: 2,502 triangles Obstacles: 432 Triangles SBL  17 sec A-SBL  4.8 sec SBL: PRM planner (single-query, bi-directional, lazy in cc) with fixed-discretization collision checker A-SBL: Same planner, with new collision checker Experiment: Run SBL 10 times on same planning problem with some resolution  If a collision has been missed, reduce  and repeat If no collision has been missed, return average planning time Run A-SBL 10 times and return average planning time

Some Results Robot: 2,502 triangles Obstacles: 432 Triangles SBL  17 sec A-SBL  4.8 sec Robot: 2,991 triangles Obstacles: 74,681 triangles SBL  1.20 sec A-SBL  0.81 sec Robot: 2,991 triangles Obstacles: 432 Triangles SBL  83 sec A-SBL  44 sec Robot: 2,502 triangles Obstacles: 34,171 triangles SBL  3.2 sec A-SBL  2.1 sec Robots: 6 x 2,991 triangles Obstacles: 19,668 triangles SBL  85 sec A-SBL  52 sec

Conclusion New collision checker suited for PRM planners:  Faster than fixed-resolution checkers  Fully reliable Future work:  Automatic computation of tight upper bounds on path lengths in w-space from robot kinematics  Better treatment of pairs of moving bodies (e.g., bodies moving along parallel paths)

Acknowledgements This research was partially funded by grants from General Motors and ABB Additional geometric models were provided by PSA (Peugeot-Citroen)

Greedy Computation of  ij (q) Algorithm GREEDY-DIST(Ba,Bb) 1.h  distance(Ba,Bb) 2.If Ba and Bb are both triangles then return h 3.If h > 0 then return h 4.If Ba is “bigger” than Bb then switch Ba and Bb 5.   GREEDY-DIST(Ba,Bb 1 ) 6.If  > 0 then 1.   GREEDY-DIST(Ba,Bb 2 ) 2.If  > 0 then return min{ ,  } 7.Return 0 Ba Bb GREEDY-DIST  is small factor slower than a pure collision checker  is much faster than BV-based exact or approximate distance computation  returns lower-bounds that are often much larger than ½ actual distances Idea: BV hierarchy + same recursion as for pure collision checking, but compute distance between boxes (  BVs are RSSs)  ij (q)