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

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

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

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 ♦ Using Graphics Hardware Hoff et al. 1999

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 ♦ Random Sampling Pisula et al MAPRM: Wilmarth et al. 1999

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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 ) Apply impulse based rigid body dynamics to resolve penetrations

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

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

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

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

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

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

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

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]

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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