Exact Collision Checking of Robot Paths

1. Exact Collision Checking of Robot Paths Fabian Schwarzer Mitul SahaJean-Claude Latombe Computer Science Department Stanford University

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

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

4. e Motivation (3) Butdynamic collision tests (for connections) using BV hierarchies are usually approximate. • e too large  collisions are missed • e too small  slow test of local paths

5. e Motivation (3) Butdynamic collision tests (for connections) using BV hierarchies are usually approximate.

6. 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.

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

8. q3 q2 q1 • Ideas: • Relate configuration changes to path lengths in workspace • Use distance computation rather than pure collision checking q = (q1,q2,q3) q’ = (q’1,q’2,q’3) dqi = q’i-qi For any q and q’ no robot point traces a path longer than: l(q,q’)= 3|dq1|+2|dq2|+|dq3|

9. q3 q2 q1 • Ideas: • Relate configuration changes to path lengths in workspace • Use distance computation rather than pure collision checking h(q)= Euclidean distance between robot and obstacles (or lower bound) h(q) If l(q,q’) < h(q) + h(q’) then the straight path betweenq and q’ is collision-free

10. q’ q {q” |l(q’,q”) < h(q’)} {q” |l(q,q”) < h(q)} • Ideas: • Relate configuration changes to path lengths in workspace • Use distance computation rather than pure collision checking l(q,q’) < h(q) + h(q’)

11. Ideas: • Relate configuration changes to path lengths in workspace • Use distance computation rather than pure collision checking l(q,q’) >h(q) + h(q’) Bisection q’ q {q” |l(q’,q”) < h(q’)} {q” |l(q,q”) < h(q)}

12. Greedy Distance Computation • Use BV hierarchy + same recursion as for pure collision checking • But compute distance between BVs instead of testing BV overlap •  BVs are RSSs • 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

13. q3 q2 q1 Generalization • Robot(s) and static obstacles treated as collection of rigid bodies A1, …, An. • li(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’ l1(q,q’) = |dq1| l2(q,q’) = 2|dq1|+|dq2| l3(q,q’) = 3|dq1|+2|dq2|+|dq3|

14. Generalization • Robot(s) and static obstacles treated as collection of rigid bodies A1, …, An. • li(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 li(q,q’) + lj(q,q’) < hij(q) + hij(q’)then Ai and Aj do not collide between q and q’

15. Generalized Bisection Method Each pair of bodies is checked independently of the others  priority queue Q of elements [qa,qb]ij Initially, Q consists of [q,q’]ij for all pairs of bodies Ai and Aj that need to be tested. • Until Q is not empty do: • [qa,qb]ij remove-first(Q) • If li(qa,qb) + lj(qa,qb) hij(qa) + hij(qb) then • qmid (qa+qb)/2 • If hij(qmid) = 0 then return collision • Else insert [qa,qmid]ij and [qmid,qb]ij into Q • Return no collision

16. Heuristic Ordering Q • Goal: Discover collision quicker if there is one. • Sort Q by decreasing values of:[li(qa,qb) + lj(qa,qb)] – [hij(qa) + hij(qb)] • Possible extension to multi-segment paths(very useful with lazy collision-checking PRM)

17. Segment Covering Strategies Allows caching of forward kinematic results

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

19. Robot in a Cage Robot: 2,991 trianglesCage: 432 triangles

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

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

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

23. 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)

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

25. Greedy Computation of hij(q) Idea: BV hierarchy + same recursion as for pure collision checking, but compute distance between boxes ( BVs are RSSs) • Algorithm GREEDY-DIST(Ba,Bb) • h distance(Ba,Bb) • If Ba and Bb are both triangles then return h • If h > 0 then return h • If Ba is “bigger” than Bb then switch Ba and Bb • a GREEDY-DIST(Ba,Bb1) • If a > 0 then • b GREEDY-DIST(Ba,Bb2) • If b > 0 then return min{a,b} • Return 0 Ba Bb hij(q) • 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