“Adaptive Dynamic Collision Checking for Single and Multiple Articulated Robots in Complex Environments” Schwarzer, Saha, and Latombe Presentation by: Eric Ng CS326A: Paper Review Spring 2003

Static Collision Checking Static collision checking tests a single configuration of objects for overlaps. AjAj Ai(q)Ai(q)

Dynamic Collision Checking Dynamic collision checking ensures that all configurations between A and B are collision-free. Fixed Resolution checking is a sequence of static BVH checks along path of motion, divided into  length segments.  is user defined and is function of accuracy and calculation time.

Dynamic Collision Checking Dynamic collision checking ensures that all configurations between A and B are collision-free. Fixed Resolution checking is a sequence of static BVH checks along path of motion, divided into  length segments.  is user defined and is function of accuracy and calculation time. A B

Fixed Resolution Collision Checker Dynamic collision checking ensures that all configurations between A and B are collision-free. Fixed Resolution checking is a sequence of static BVH checks along path of motion, divided into  length segments.  is user defined and is function of accuracy and calculation time. A B

Fixed Resolution Collision Checker Dynamic collision checking ensures that all configurations between A and B are collision-free. Fixed Resolution checking is a sequence of static BVH checks along path of motion, divided into  length segments.  is user defined and is function of accuracy and calculation time. A B

Fixed Resolution Collision Checker Dynamic collision checking ensures that all configurations between A and B are collision-free. Fixed Resolution checking is a sequence of static BVH checks along path of motion, divided into  length segments.  is user defined and is function of accuracy and calculation time.  A B

Drawbacks of Fixed Resolution Checking (1)  determines accuracy. If  isn’t fine enough, collision at a point between checks will not be detected. (2) Refining resolution improves accuracy, but calculation becomes more expensive. q1 q2  q1 q2 Undetected obstacle

Why Adaptive Dynamic Checking? Adaptive Dynamic Checking offers: (1) Ability to never miss collisions. (2) And not costing much more than classical collision checking. (3) Has one drawback: requires calculation of distances between objects, not just collision checking => can be computationally expensive. (Ameliorated with optimizations!)

Basis for Adaptive Dynamic Collision Checking The robot and obstacles are defined by: A 1,…,A n, whose placement in workspace is q=(q 1,…,q n ) AjAj Ai(q)Ai(q)

Basis for Adaptive Dynamic Collision Checking n ij (q) is lower bound distance between A i and A j  (q) AjAj Ai(q)Ai(q)

Basis for Adaptive Dynamic Collision Checking n ij (q) is lower bound distance between A i and A j  (q) AjAj Ai(q)Ai(q) A i (q 1 ) i (q 1, q 2 ) A j (q 2 ) i (q 1, q 2 ) is upper bound path length of A i from config 1 to 2.

Basis for Adaptive Dynamic Collision Checking Ai qaqa qbqb i (q a,q b ) qbqb qaqa j (q a,q b ) Aj If i (q a,q b ) + j (q a,q b ) < n ij (q a ) + n ij (q b ), then there exists no collisions between q a and q b n ij (q a ) n ij (q b ) Lemma

What if Inequality Fails? It does not mean there is a collision between “a” and “b”!! Inequality only determines non-collisions. Collision is defined by zero distance between Ai and Aj, ie: n ij =0. Ai qaqa qbqb qbqb qaqa Aj n ij (q b )=0

Example of Adaptive Bisection of Paths Run Inequality test on path segment. [PASS] = no collisions between q a and q b qaqa qbqb

Example of Adaptive Bisection of Paths 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 qmid doesn’t have a collision, recursively test both sub-segments until collision is found, or both sub- segments pass inequality test ==> never misses collision! qaqa qbqb q mid Run Inequality Test

Example of Adaptive Bisection of Paths 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 qmid doesn’t have a collision, recursively test both sub-segments until collision is found, or both sub- segments pass inequality test ==> never misses collision! qaqa qbqb q mid Run Inequality Test

Calculating the terms in the Inequality i (q a,q b ) + j (q a,q b ) < n ij (q a ) + n ij (q b ) But how do we get n ij and i (q a,q b )? A i (q 1 ) i (q 1, q 2 ) A j (q 2 )  (q) AjAj Ai(q)Ai(q)

Computing Lower Bound Dist. Betw. Objects: n ij Algorithm GREEDY-DIST(Bi, Bj)  = distance(Object i,Object j) –If  > 0, then return  –Else, we need to investigate further Object i Half,i1 Leaf,i1Leaf,i2 Half,i2 Object j Half,j1 Leaf,j1Leaf,j2 Half,j2

Computing Lower Bound Dist. Betw. Objects: n ij Algorithm GREEDY-DIST(Bi, Bj) Object i Half,i1 Leaf,i1Leaf,i2 Half,i2 Object j Half,j1 Leaf,j1Leaf,j2 Half,j2 Test both Half,j1 and Half,j2 against Object i –Dist1 = Greedy-Dist(Object I, Half,j1) –Dist2 = Greedy-Dist(Object I, Half,j2) Dist2 > 0 (stop in that search path) Dist1 !> 0 (need to continue further)  = 0  > 0  = 0

Computing Lower Bound Dist. Betw. Objects: n ij Algorithm GREEDY-DIST(Bi, Bj) Object i Half,i1 Leaf,i1Leaf,i2 Half,i2 Object j Half,j1 Leaf,j1Leaf,j2 Half,j2  = 0  > 0  = 0 Test Half,j1 against Half,i1 and Half,i2 –Dist1 = Greedy-Dist(Half,j1, Half,i1) –Dist2 = Greedy-Dist(Half,j1,Half,i2) Both Dist1 and Dist 2 > 0 => DONE! Return min(dist1, dist2) as n ij

Example of Bounding Motion Calculation: i q4 q2 q1 q3 A1 A2 A3 A4 D: dist by q3 L: length of each rigid body 4 d.o.f. robot 3 rotational and 1 prismatic joint D is the max distance travelled by Q3 L is the length of each rigid body

Example of Bounding Motion Calculation: i q1 A1 Upper Bound Path LengthRigid Body A1

Example of Bounding Motion Calculation: i q2 q1 A1 A2 Upper Bound Path LengthRigid Body A1 A2 A3 A4

Example of Bounding Motion Calculation: i q2 q1 q3 A1 A2 A3 Upper Bound Path LengthRigid Body A1 A2 A3 A4

Example of Bounding Motion Calculation: i q4 q2 q1 q3 A1 A2 A3 A4 Upper Bound Path LengthRigid Body A1 A2 A3 A4

Conclusions Adaptive Bisection Collision Checker is an efficient and robust way to determine if a proposed path is collision-free. Even though it is efficient, there is room for improvement. For example, path lengths are overly conservatively computed. Bisection method may not be most efficient solution when obstacle is always closeby the robot.

Optimizing the Process 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.

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 Slide courtesy Latombe,2003

More 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

Summary Adaptive bisection collision checker is a reliable and effective way to test straight line segments. Advantages over fixed resolution checker: –Never miss a collision –Eliminates need for determining resolution factor:  Determines whether subsections (“i” and “j”) of objects collide when moving from configuration “a” to “b” i (q a,q b ) + j (q a,q b ) < n ij (q a ) + n ij (q b ), does not collide –Requires lower bound distances between two bounding volumes (BVs) i & j at configs “a” & “b” –Requires upper bound distances in path lengths of objects i & j from config “a” to “b” Inequality is performed on BVH binary tree until collision is found, or tree is fully traversed…leading to no collisions found