Deadlock-Free and Collision-Free Coordination of Two Robot Manipulators Patrick A. O’Donnell and Tomas Lozano-Perez ’89 Presented by Vishal Srivastava Slides by Huy Nguyen with additions and modifications by Vishal Srivastava

Introduction Goals Coordinate the trajectories of two robot manipulators so as to avoid collisions and deadlock. Minimize total execution time Definitions path – Curve in C-space trajectory – Time history of positions along a path

Assumptions  Environment is known by both robots  Individual paths are planned off-line prior to coordination  Paths are predictable; trajectories are less predictable

The Approach  Decouple path specification step from trajectory specification step.  Each individually-planned path is composed of a sequence of path segments.  We estimate the time required to execute each segment. Trajectory coordination problem becomes a scheduling problem where space is the shared resource.

Task-Completion (TC) Diagram B A sBsB sAsA gBgB gAgA sBsB gBgB sAsA gAgA Paths in C-Space Task-Completion Diagram

Task-Completion (TC) Diagram B A sBsB sAsA gBgB gAgA

B A sBsB sAsA gBgB gAgA  Axes represent robot path segments.

Task-Completion (TC) Diagram B A sBsB sAsA gBgB gAgA  Axes represent robot path segments.  Rectangle Rij is shaded if the swept volume of the ith path segment of A intersects with the swept volume of the jth path segment of B.

Task-Completion (TC) Diagram B A sBsB sAsA gBgB gAgA  Axes represent robot path segments.  Rectangle Rij is shaded if the swept volume of the ith path segment of A intersects with the swept volume of the jth path segment of B.  A schedule is a non-decreasing curve that connects the lower-left corner of diagram to the top-right corner.

Task-Completion (TC) Diagram B A sBsB sAsA gBgB gAgA  Axes represent robot path segments.  Rectangle Rij is shaded if the swept volume of the ith path segment of A intersects with the swept volume of the jth path segment of B.  A schedule is a non-decreasing curve that connects the lower-left corner of diagram to the top-right corner.  A safe schedule is a schedule that never penetrates the interior of the union of collision rectangles.

Task-Completion (TC) Diagram A B sAsA sBsB gAgA gBgB  Axes represent robot path segments.  Rectangle Rij is shaded if the swept volume of the ith path segment of A intersects with the swept volume of the jth path segment of B.  A schedule is a non-decreasing curve that connects the lower-left corner of diagram to the top-right corner.  A safe schedule is a path that never penetrates the interior of a collision rectangle.  Boundaries of collision rectangles are safe!

Greedy Scheduler B A sBsB sAsA gBgB gAgA procedure Greedy Scheduler; begin i:=0; j:=0; while i < m or j < n do begin if R i,j is collision free then begin if i < m then begin Execute A i ; i:=i+1; end if j < n then begin Execute B j ; j:=j+1; end end else if i < m and R i,j-1 is collision free then begin Execute Ai; i:=i+1; end else if j < n and R i-1,j is collision free then begin Execute Bj; j:=j+1; end Wait for any completion signals; end

Greedy Scheduler B A sBsB sAsA gBgB gAgA procedure Greedy Scheduler; begin i:=0; j:=0; while i < m or j < n do begin if R i,j is collision free then begin if i < m then begin Execute A i ; i:=i+1; end if j < n then begin Execute B j ; j:=j+1; end end else if i < m and R i,j-1 is collision free then begin Execute Ai; i:=i+1; end else if j < n and R i-1,j is collision free then begin Execute Bj; j:=j+1; end Wait for any completion signals; end

Greedy Scheduler B A sBsB sAsA gBgB gAgA procedure Greedy Scheduler; begin i:=0; j:=0; while i < m or j < n do begin if R i,j is collision free then begin if i < m then begin Execute A i ; i:=i+1; end if j < n then begin Execute B j ; j:=j+1; end end else if i < m and R i,j-1 is collision free then begin Execute Ai; i:=i+1; end else if j < n and R i-1,j is collision free then begin Execute Bj; j:=j+1; end Wait for any completion signals; end

Deadlock B A sBsB sAsA gBgB gAgA  Greedy Scheduler can become Deadlocked.

SW-closure. B A sBsB sAsA gBgB gAgA  Avoid deadlock by computing SW- closure of union of collision regions to fills in non-convexities.  After taking the SW-closure… A schedule exists if and only if both the origin and goal remain clear.

Increasing Parallelism  Parallelism is the degree of concurrency with which the paths can be executed  Assume segment lengths now corresponds to expected execution time  Best-planned paths have high parallelism  Strive for a path close to the diagonal B A

Increasing Parallelism A  TC Diagram may have collision regions near diagonal because of original choice of paths. B

Increasing Parallelism B A B A  For a problematic collision region, replan the path of A treating the volume swept by B as an obstacle. Replanned path of A will typically be longer.

Conclusions  Main Ideas Decoupling of path and trajectory planning. Formulation of coordination as a scheduling problem, use of Task-Completion diagram, etc. Replans path to increase parallelism only in problem regions using space-time planning.  Concerns How will it work for robots with multiple moving joints? Many approximations along the way. Too conservative? No precise coordination. No experimental data. Any implementations?

Robots A, B, C, …?  Could this be extended for >2 robots?  N-dimensional TC-Diagrams  Number of manipulators colliding in a region varies. Can make use of the degree of collision when deciding on which path segments to replan?  More ideas?

Backtracking?  Glaring omission: ability to go backwards along the path  Paths would be unchanged, but velocity of trajectory could be negative  Search for safe schedule becomes more difficult  SW-Closure would eliminate solutions  Only worthwhile if such an interaction is anticipated B A ???