Adaptive Dynamic Collision Checking for Many Moving Bodies

1. Adaptive Dynamic Collision Checking for Many Moving BodiesMitul SahaDepartment of Computer Science,Stanford University. NSF-ITR Workshop Collaborators: Fabian Schwarzer, Jean-Claude Latombe

2. Static vs. Dynamic Collision Checking Static Dynamic • Static collision checking tests a single configuration q for collision • Dynamic collision checking ensure that all configurations on a continuous path are collision-free.

3. Motivation (1) Bottleneck in PRM planning: Efficient testing of sampled configurations and connections for collision. qb qa (Usually people trade-off reliability for speed in dynamic collision checking.)

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

5.  Motivation (3) Butdynamic collision tests (for connections betweensampled configurations) using BV hierarchies are usually approximate. 3 2 3 1 3 2 3 •  too large  collisions are missed •  too small  slow test of local paths

6.  Motivation (3) Butdynamic collision tests (for connections betweensampled configurations) using BV hierarchies are usually approximate.

7. 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 Sequential strategy (first collision) that is not efficient for PRM path segments • Swept-volume intersection[Cameron, 85] [Foisy, Hayward, 93] Swept-volumes are expensive to compute. Too much data. No pre-computed BV hierarchies • Algebraic trajectory parameterization[Canny, 86] [Schweikard, 91] [Redon, Kheddar, Coquillard, 00] High-degree polynomials, expensive • Combination[Redon, Kheddar, Coquillard, 00] BVH + algebraic parameterization [Ehmann, Lin, 01] BVH + feature tracking Sequential strategy

8. Desirables in Nutshell • Adaptive resolution • Exact collision checking • Competitive with discrete checkers w.r.t. speed

9. Definitions Workspace C-space Ai (qb) qb i (qa,qb) qb qa qa Ai (qa) • i(qa,qb)≡curveLength(Ai) on qa…qb

10. Definitions Workspace C-space Ai (qb) Aj (qb) • ij(q)≡distance(Ai, Aj)  • ij(q)= 0  Collision qb ηij (qb) qb qa qa

11. Key Lemma: If i (qa,qb) + j (qa,qb) < ηij (qa) + ηij (qb), then there exists no collision between Ai and Aj along qa-qb Key Lemma Workspace C-space Ai (qb) Aj (qb) qb ηij (qb) i (qa,qb) j (qa,qb) qb qa ηij (qa) qa Aj (qa) Ai (qa)

12. Key Lemma: If i (qa,qb) + j (qa,qb) < ηij (qa) + ηij (qb), then there exists no collision between Ai and Aj along qa-qb • Intuition behind the key lemma: Key Lemma Aj i Ai ηij i < ηijSafe

13. i (qa,qb) + j (qa,qb) < ηij (qa) + ηij (qb) Collision -free If i (qa,qb) + j (qa,qb) ηij (qb) ηij (qa) else Not sure! Hence bisect Collision Checking using the Lemma

14. Collision Checking using the Lemma • After the collision check a collision-free connection may look like: • Each sub-segment segment satisfies the lemma: i (qa,qb) + j (qa,qb) < ηij (qa) + ηij (qb) and hence the collision checking is exact • The collision checking is adaptive

15. Generalization:Large Number of Moving Bodies • Robot(s) and static obstacles treated as collection of rigid bodies A1, …, An. • If i(q,q’) + j(q,q’) < ij(q) + ij(q’)then Ai and Aj do not collide between q and q’

16. Generalized Bisection Method for Large Number of Moving Objects • 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 i(qa,qb) + j(qa,qb) ij(qa) + ij(qb) then • qmid (qa+qb)/2 • If ij(qmid) = 0 then return collision • Else insert [qa,qmid]ij and [qmid,qb]ij into Q • Return no collision

17. 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)]

18. Generalized Bisection Method for Large Number of Moving Bodies • Things to note: • Different pair of bodies tested with different resolutions, i.e. usually close pair of objects are checked with finer resolution than the distant ones. • Usually close pair of objects are considered before the distant ones.

19. i (qa,qb) + j (qa,qb) < ηij (qa) + ηij (qb) How do we getij(q)andi(qa,qb)? ij(q) Aj(q) Calculating Terms in the Key Lemma i(qa,qb) Ai(q) Ai(qa) Aj(qb)

20. Lower bound on Distance between Bodies • Need to find ηij’s • Exact and approximate distance computations are very expensive compared to pure collision checking • So is there any alternative? Aj Aj ηij

21. Lower bound on Distance between Bodies • Use a classical BVH collision checker with a slight modification • Compute distance between BVs instead of just testing BV overlap  BVs are RSSs ij(q) • returns values often much larger than ½ distances • much faster than BV-based exact or ½-approximate distance computation • almost as fast as pure collision checking

22. i (qa,qb) + j (qa,qb) < ηij (qa) + ηij (qb) How do we getij(q)andi(qa,qb)? ij(q) Aj(q) Calculating Terms in the Key Lemma i(qa,qb) Ai(q) Ai(qa) Aj(qb)

23. A4 q4    ( q , q ) L q q 1 a b a , 1 b , 1 A3      ( q , q ) 2 L q q L q q 2 a b a , 1 b , 1 a , 2 b , 2 q3 q2     ( q , q ) ( 2 L D ) q q 3 a b a , 1 b , 1 A2      ( L D ) q q q q a , 2 b , 2 a , 3 b , 3 A1     ( q , q ) ( 3 L D ) q q q1 4 a b a , 1 b , 1      ( 2 L D ) q q q q a , 2 b , 2 a , 3 b , 3   L q q a , 4 b , 4 Bounding Motions in Workspace i(qa,qb) Rigid Body Upper Bound Path Length A1 A2 A3 A4 

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

25. Some Result 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

26. In Action

27. In Action

28. Summing up.. • New dynamic collision checker which: • Never misses a collision • Eliminates need for determining resolution factor: e • Is even faster than the discrete checker in PRM planning • Techniques that considerably improve efficiency: • Greedy Distance Computation algorithm, nearly as efficient as pure collision checker • Fast technique for bounding lengths of paths traced out in workspace • Heuristics for ordering collisions tests • Represent deformable objects with a large number of small rigid objects vs

29. Summing up.. • A free downloadable c++ implementation of the new dynamic collision checker is on the web: http://robotics.stanford.edu/~mitul/mpk

30. NSF-ITR Workshop Collaborators: Fabian Schwarzer, Jean-Claude Latombe Adaptive Dynamic Collision Checking for Many Moving ObjectsMitul SahaDepartment of Computer ScienceStanford University Any Question?

31. SBL X [Sanchez-Ante, Latombe, 01]

32. mg mb SBL [Sanchez-Ante, Latombe, 01]

33. Our New Collision Checker • Based on “Adaptive Bisection” • Ideas: • Relate configuration changes to path lengths in workspace • Use distance computation rather than pure collision checking

34. Current Research • Extension to deformable objects • Large number of small bodies can be used to approximate • deformable objects