Adaptive Dynamic Collision Checking for Many Moving Bodies Mitul Saha Department of Computer Science, Stanford University. NSF-ITR Workshop Collaborators: Fabian Schwarzer, Jean-Claude Latombe

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

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

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

But dynamic collision tests (for connections between sampled configurations) 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 between sampled configurations) using BV hierarchies are usually approximate.

Previous Approaches to Dynamic Collision Testing o Bounding-volume (BV) hierarchies  Discretization issue o 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 o Swept-volume intersection [Cameron, 85] [Foisy, Hayward, 93]  Swept-volumes are expensive to compute. Too much data.  No pre-computed BV hierarchies o Algebraic trajectory parameterization [Canny, 86] [Schweikard, 91] [Redon, Kheddar, Coquillard, 00]  High-degree polynomials, expensive o Combination [Redon, Kheddar, Coquillard, 00] BVH + algebraic parameterization [Ehmann, Lin, 01] BVH + feature tracking  Sequential strategy

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

Definitions A i (q a ) WorkspaceC-space A i (q b ) qaqa qbqb qaqa i (q a,q b )  i (q a,q b ) ≡  curveLength (A i ) on q a … q b  qbqb

Definitions WorkspaceC-space A i (q b ) A j (q b ) qaqa qbqb qbqb qaqa η ij (q b )   ij (q) ≡  distance (A i, A j )    ij (q) = 0  Collision

Key Lemma A i (q a ) WorkspaceC-space A j (q a ) A i (q b ) A j (q b ) qaqa qbqb qbqb qaqa η ij (q a ) η ij (q b ) i (q a,q b ) j (q a,q b ) Key Lemma: If i (q a,q b ) + j (q a,q b ) < η ij (q a ) + η ij (q b ), then there exists no collision between A i and A j along q a -q b

Key Lemma Key Lemma: If i (q a,q b ) + j (q a,q b ) < η ij (q a ) + η ij (q b ), then there exists no collision between A i and A j along q a -q b Intuition behind the key lemma: η ij i AiAi AjAj i < η ij Safe

Collision Checking using the Lemma Hence bisect Not sure! else If i (q a,q b ) + j (q a,q b ) < η ij (q a ) + η ij (q b ) Collision -free η ij (q a ) η ij (q b ) i (q a,q b ) + j (q a,q b )

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

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

Generalized Bisection Method for Large Number of Moving Objects 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 o Each pair of bodies is checked independently of the others  priority queue Q of elements [q a,q b ] ij o Initially, Q consists of [q,q’] ij for all pairs of bodies Ai and Aj that need to be tested.

Heuristic Ordering Q o Goal: Discover collision quicker if there is one. o Sort Q by decreasing values of: [ i (q a,q b ) + j (q a,q b )] – [  ij (q a ) +  ij (q b )]

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

Calculating Terms in the Key Lemma How do we get  ij (q) and i (q a,q b ) ? i (q a,q b ) + j (q a,q b ) < η ij (q a ) + η ij (q b ) A j (q) i (q a, q b )  ij (q) A j (q b ) A i (q)A i (q a )

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

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

Calculating Terms in the Key Lemma How do we get  ij (q) and i (q a,q b ) ? i (q a,q b ) + j (q a,q b ) < η ij (q a ) + η ij (q b ) A j (q) i (q a, q b )  ij (q) A j (q b ) A i (q)A i (q a )

Bounding Motions in Workspace i (q a,q b ) q3q3 A3A3 q2q2 A2A2 q4q4 A4A4 q1q1 A1A1 Upper Bound Path LengthRigid Body A1A1  1 (q a,q b )  Lq a,1  q b,1 2 (q a,q b )  2Lq a,1  q b,1  Lq a,2  q b,2 3 (q a,q b )  (2L  D)q a,1  q b,1  (L  D)q a,2  q b,2  q a,3  q b,3 4 (q a,q b )  (3L  D)q a,1  q b,1  (2L  D)q a,2  q b,2  q a,3  q b,3  Lq a,4  q b,4 A2A2 A3A3 A4A4

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 Result 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

In Action

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

Summing up.. A free downloadable c++ implementation of the new dynamic collision checker is on the web:

Adaptive Dynamic Collision Checking for Many Moving Objects Mitul Saha Department of Computer Science Stanford University NSF-ITR Workshop Collaborators: Fabian Schwarzer, Jean-Claude Latombe Any Question?

SBL [Sanchez-Ante, Latombe, 01] X

SBL mbmbmbmb mgmgmgmg

Our New Collision Checker oBased on “Adaptive Bisection” oIdeas: oRelate configuration changes to path lengths in workspace oUse distance computation rather than pure collision checking

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