Adaptive Dynamic Collision Checking for Single and Multiple Articulated Robots in Complex Environments Schwarzer, Saha, and Latombe CS326A Winter 2004, Presented by Irena Pashchenko

Static vs. Dynamic Collision Checking Static checking tests a single configuration q for overlaps A j (q) A i (q)

Static vs. Dynamic Collision Checking Dynamic checking ensure that all configurations on a continuous path are collision-free. qaqa qbqb

Fixed Resolution Methods Fixed Resolution checking is a sequence of static BVH checks along path of motion, divided into  length segments. qaqa qbqb 

Fixed Resolution Methods  is user selected value. Compromise between efficiency and reliability. qaqa qbqb 

Adaptive Dynamic Collision Checking Automatic local adjustment of value  Collisions are never missed Does not cost much more than classical collision checking. Requires calculation of distances between objects; typically more expensive than plain collision checking. But paper proposed new techniques and heuristics to speed this up. Applicable to straight path segments in c-space Suited for scenarios with manipulator arms and/or multiple robots

Definitions The robot and obstacles are defined by: A 1,…,A n, whose placement in workspace is q=(q 1,…,q n ) A j (q) A i (q) i (q a, q b )  ij (q) A i (q a ) A j (q b )  ij (q) ≡  distance (A i, A j )  i (q a,q b ) ≡  curveLength (A i ) on q a … q b 

Key Lemma for Adaptive Bisection AiAi qaqa qbqb i (q a,q b ) qbqb qaqa j (q a,q b ) AjAj Lemma: If i (q a,q b ) + j (q a,q b ) < η ij (q a ) + η ij (q b ), then there exists no collisions between A i (q a ) and A j (q b ) η ij (q a ) η ij (q b )

Key Lemma for Adaptive Bisection Inequality only determines non-collisions. AiAi qaqa qbqb qaqa AjAj η ij (q b ) = 0 Collision is defined by zero distance between A i and A j, i.e. η ij = 0.

Adaptive Bisection of Paths Example qaqa qbqb q mid Run Inequality Tests Run Inequality test on path segment. [PASS] = no collisions between q a and q b [FAIL] = check if robot makes collision at q mid If q mid doesn’t have a collision, recursively test both sub- segments until collision is found, or both sub-segments pass inequality test  Collisions are never missed!

Optimizations Let Q be the priority queue containing elements objects being checked. If the path segment is collision-free, then the ordering of Q has no impact on running time. But for a colliding path, the calculation can complete as soon as collision is found => ordering matters. Since longer path lengths have higher probability of collision, Q is presorted by decreasing path lengths to potentially find collisions earlier.

Calculating Terms in the Inequality 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) A i (q) i (q a, q b )  ij (q) A i (q a ) A j (q b )

Algorithm GREEDY-DIST(B i,B j ) d = distance(B i, B j ) If triangles or d > 0, then return d  = GREEDY-DIST( B i, B j.childLeft ) if(  > 0 )  = GREEDY-DIST(B i, B j.childRight ) if(  > 0 ) return min { ,  } return 0 Object i Half,i1 Leaf,i1Leaf,i2 Half,i2 Object j Half,j1 Leaf,j1Leaf,j2 Half,j2 Lower Bound On Distance Betw. Objects:  ij (q) Rectangle Swept Spheres (RSS)

Bounding Motions in Workspace i (q a,q b ) q4q4 q2q2 q1q1 q3q3 A1A1 A2A2 A3A3 A4A4 4 d.o.f. robot Joints: 3 rotational and 1 prismatic D is the max distance traveled by q 3 L is the length of each rigid link

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

Summary – Adaptive Bisection Collision Checker Efficient and robust way to determine collision-free paths. Advantages over fixed resolution checker: Never misses a collision Eliminates need for determining resolution factor:  There is room for improvement. Derive tighter bounds on path lengths. 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

Summary Limitations: Not the best method if one wants to determine the first collision configuration moving from q a to q b Bad-case scenario: two moving objects stay very close along a long section of a path segment. (The key inequality fails to do its job here, and does not sample the path segments at coarser resolution.)