1. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Constraint-Based Motion Planning using Voronoi Diagrams Maxim Garber and Ming C. Lin Department of Computer Science http://gamma.cs.unc.edu/cplan/

2. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Introduction A motion planning method ♦ for rigid and articulated objects ♦ in dynamic environments ♦ using Voronoi Diagrams Allowing incorporation of various geometric, physical and mechanical constraints

3. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Previous Work Roadmap Based Planning ♦ Randomized PRM: Kavraki & Latombe 1994, Kavraki et al. 1996 OBPRM: Amato et al. 1998 MAPRM: Wilmarth et al. 1999 ♦ Voronoi Based Ó Dúnlaing 1983 Choset et al. 1995, 1996 vPlan: Foskey et al. 2001

4. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Previous Work Motion Planning in Dynamic Environments ♦ Artificial Potential Fields Khatib 1986 ♦ Industrial Applications Ahrentsen et al. 1997 ♦ Using Graphics Hardware Hoff et al. 1999

5. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Previous Work Voronoi Diagrams in Motion Planning ♦ Voronoi Graph Ó Dúnlaing 1983 Choset et al. 1995, 1996 vPlan: Foskey et al. 2001 ♦ Random Sampling Pisula et al. 2000 MAPRM: Wilmarth et al. 1999

6. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Basic Approach Characteristics: Reactive Planning -- handling dynamic scenes and moving obstacles/robots

7. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Basic Approach Characteristics: Reactive Planning -- handling dynamic scenes and moving obstacles/robots Estimated Roadmap -- providing global information through estimated paths

8. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Basic Approach Characteristics: Reactive Planning -- handling dynamic scenes and moving obstacles/robots Estimated Roadmap -- providing global information through estimated paths Voronoi Diagrams -- capturing a useful characterization of workspace

9. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Basic Approach Characteristics: Reactive Planning -- handling dynamic scenes and moving obstacles/robots Estimated Roadmap -- providing global information through estimated paths Voronoi Diagrams -- capturing a useful characterization of workspace …… combine these in a general and extensible constraint-based motion planning framework

10. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Framework Objectives Portable ♦ Handle rigid, articulated, and deformable (future work) robots

11. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Framework Objectives Portable ♦ Handle rigid, articulated, and deformable (future work) robots Dynamic ♦ Allow scenes with dynamic obstacles

12. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Framework Objectives Portable ♦ Handle rigid, articulated, and deformable (future work) robots Dynamic ♦ Allow scenes with dynamic obstacles General ♦ Allow a wide range of relationships between objects to be specified

13. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Planning Framework Formulate motion planning as a constrained dynamical system Introduce both hard and soft constraints ♦ guide the robot(s) to their goal(s) ♦ avoiding collision with other robot(s) and obstacles

14. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Framework Example Environment contains obstacles The obstacles may be dynamic

15. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL The robot is a collection of rigid objects Each rigid object has state: position rotation linear velocity angular velocity Framework Example

16. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL The objects are subject to various constraints. Constraints that define the problem: Non-Penetration Framework Example

17. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL The objects are subject to various constraints. Constraints that define the problem: Non-Penetration Joint Connectivity Framework Example

18. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL The objects are subject to various constraints. Constraints that define the problem: Non-Penetration Joint Connectivity Joint Angle Limits Framework Example

19. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Given a planning goal Define constraints that encourage planning behavior: Framework Example

20. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Given a planning goal Define constraints that encourage planning behavior: Estimated Path Framework Example

21. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Given a planning goal Define constraints that encourage planning behavior: Estimated Path Obstacle Avoidance Framework Example

22. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Simulation Loop: Framework Example

23. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Simulation Loop: Update Obstacles Framework Example

24. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Simulation Loop: Update Obstacles Framework Example

25. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Simulation Loop: Update Obstacles Apply Planning Constraints Framework Example

26. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Framework Example Simulation Loop: Update Obstacles Apply Planning Constraints

27. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Simulation Loop: Update Obstacles Apply Planning Constraints Enforce Problem Constraints Framework Example

28. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Simulation Loop: Update Obstacles Apply Planning Constraint Forces Enforce Problem Constraints Repeat Until Goal is Achieved Framework Example

29. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL General Framework Simulation Loop

30. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL General Framework Simulation Loop Robots, Obstacles, Goals …

31. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL General Framework Simulation Loop Robots, Obstacles, Goals … Constraints C1C1 C2C2 C3C3 …

32. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL General Framework Simulation Loop INPUT: Robots, Obstacles, Goals … Constraints C1C1 C2C2 C3C3 … Constraint ForceEnergy Function

33. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL General Framework Simulation Loop Robots, Obstacles, Goals … Constraints C1C1 C2C2 C3C3 … Constraint Solvers S1S1 S2S2 S3S3 …

34. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL General Framework Simulation Loop Robots, Obstacles, Goals … Constraints C1C1 C2C2 C3C3 … Constraint Solvers S1S1 S2S2 S3S3 … Run Simulation Planned Path

35. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Types of Constraints Hard Constraints Soft Constraints

36. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Hard Constraints Must be enforced throughout the entire simulation Solved using Gauss-Seidel Iteration Examples: ♦ object non-penetration ♦ joint connectivity ♦ joint angle limits

37. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Gauss-Seidel Iteration For each hard constraint we require an Instance Solver, Relax() After applying Relax(C i ) the residual of the constraint C i, Res(C i ) = 0

38. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Gauss-Seidel Iteration let S be the state of the simulation Repeat{ for each hard constraint C i { S  Relax(C i ) } } until  |Res(C i )| = 0

39. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Non-Penetration In the event of collision, prevent object penetration Use Proximity Query Package (Gottschalk et al. 1996, Larsen et al. 2000 ) Apply impulse based rigid body dynamics to resolve penetrations

40. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Joint Constraints Simple Atomic Constraints

41. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Joint Constraints Simple Atomic Constraints ♦ point distance constraint p1p1 p2p2 d

42. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Joint Constraints Simple Atomic Constraints ♦ point distance constraint ♦ point planar angle constraint p1p1 p2p2 d

43. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Residuals Simple Atomic Constraints ♦ point distance constraint ♦ point planar angle constraint

44. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Joint Constraints Combine atomic constraints to form joints Example1 : A Ball Joint

45. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Joint Constraints Example 2: A Revolute Joint Combine atomic constraints to form joints

46. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Soft Constraints Encourage planning behavior Solved using penalty forces Examples: ♦ goal seeking ♦ obstacle avoidance ♦ estimated path following

47. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Voronoi Diagrams Partition space into regions by closest primitive Discretized version can be computed quickly using graphics hardware [Hoff et al. 1999]

48. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Voronoi Diagrams Provide key planning constraints: ♦ Global Estimated Paths ♦ Local Obstacle Avoidance

49. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Estimated Paths Based On vPlan [Foskey et al. 2001] Extract estimated path from a 3D Voronoi Diagram of obstacles computed using graphics HW This estimated path can be recomputed and updated as objects in the scene move

50. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Obstacle Avoidance Distance Fields ♦ Computed in 3D ♦ A byproduct of the graphics hardware based Voronoi Diagram computation ♦ For each point in space, provide the distance to the nearest obstacle

51. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Obstacle Avoidance Example R 1 must be farther from R 2 than a specified threshold distance

52. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Obstacle Avoidance Example Localize computation using bounding boxes

53. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Obstacle Avoidance Example Compute distance field of R 2 in local region

54. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Obstacle Avoidance Example Apply forces at sample points on R 1

55. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Obstacle Avoidance Example Resultant force pushes R 1 away from R 2

56. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Obstacle Avoidance Distance Field can be recomputed every frame Applicable to deformable robots & obstacles whose shape changes every frame

57. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Results Applied to 3 planning scenes ♦ Maintainability Study ♦ Automated Car Painting ♦ Assembly Line Planning Timings Taken On: ♦ Pentium3 933MHz, 256MB RAM, NVIDIA GeForce2 GPU

58. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Maintainability Study Start Goal Scene: ♦ static environment with 2 moving robots ♦ 20,000 polygons Constraints ♦ Non-Penetration, Estimated Path, Obstacle Avoidance

59. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Maintainability Study Performance: ♦ Average Time Step 0.093 seconds ♦ Total Time 67 seconds ♦ The main bottleneck is the distance field calculation Video

60. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Automotive Painting Scene: ♦ static environment and 6 linked moving objects (robot arm) ♦ 25,000 polygons Constraints ♦ Non-Penetration, Estimated Path, Obstacle Avoidance, 40 atomic joint constraints Start Goal

61. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Automotive Painting Performance: ♦ Average Time Step 0.038 seconds ♦ Total Time 18 seconds Video

62. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Assembly Line Planning Scene: ♦ static environment, 2 moving obstacles, and 6 linked moving objects (robot arm) ♦ 17,000 polygons Constraints ♦ Non-Penetration, Goal Seeking, Obstacle Avoidance, 40 atomic joint constraints Start Goal

63. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Assembly Line Planning Performance: ♦ Average Time Step 0.0085 seconds ♦ Total Time 16 seconds Video

64. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Conclusion Planner ♦ Dynamic scenes using local constraints ♦ Global planning, using estimated path constraints ♦ Articulated objects represented using constraints

65. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Conclusion Framework ♦ Static and dynamic environments ♦ General relationships between objects ♦ Extensible to many application areas

66. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Future Work More Challenging Scenes ♦ Narrow Passages ♦ Many Dynamic Obstacles ♦ Deformable Objects

67. The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL Future Work Constraints ♦ More sophisticated constraint solver Optimization based Hybrid combination of global & local techniques ♦ More Constraint Types: Non-holonomic Line of sight Direct human interaction