Constraint-Based Motion Planning using Voronoi Diagrams

1
Constraint-Based Motion Planning using Voronoi
Diagrams

• Maxim Garber and Ming C. Lin
• Department of Computer Science
• http//gamma.cs.unc.edu/cplan/

2
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
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
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
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
Basic Approach

• Characteristics
• Reactive Planning -- handling dynamic scenes and
  moving obstacles/robots

7
Basic Approach

• Characteristics
• Reactive Planning -- handling dynamic scenes and
  moving obstacles/robots
• Estimated Roadmap -- providing global information
  through estimated paths

8
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
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
Framework Objectives

• Portable
• Handle rigid, articulated, and deformable (future
  work) robots

11
Framework Objectives

• Portable
• Handle rigid, articulated, and deformable (future
  work) robots
• Dynamic
• Allow scenes with dynamic obstacles

12
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
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
Framework Example

• Environment contains obstacles
• The obstacles may be dynamic

15
Framework Example

• The robot is a collection of rigid objects
• Each rigid object has state
• position
• rotation
• linear velocity
• angular velocity

16
Framework Example

• The objects are subject to various constraints.
• Constraints that define the problem
• Non-Penetration

17
Framework Example

• The objects are subject to various constraints.
• Constraints that define the problem
• Non-Penetration
• Joint Connectivity

18
Framework Example

• The objects are subject to various constraints.
• Constraints that define the problem
• Non-Penetration
• Joint Connectivity
• Joint Angle Limits

19
Framework Example

• Given a planning goal
• Define constraints that encourage planning
  behavior

20
Framework Example

• Given a planning goal
• Define constraints that encourage planning
  behavior
• Estimated Path

21
Framework Example

• Given a planning goal
• Define constraints that encourage planning
  behavior
• Estimated Path
• Obstacle Avoidance

22
Framework Example
Simulation Loop
23
Framework Example

• Simulation Loop
• Update Obstacles

24
Framework Example

• Simulation Loop
• Update Obstacles

25
Framework Example

• Simulation Loop
• Update Obstacles
• Apply Planning Constraints

26
Framework Example

• Simulation Loop
• Update Obstacles
• Apply Planning Constraints

27
Framework Example

• Simulation Loop
• Update Obstacles
• Apply Planning Constraints
• Enforce Problem Constraints

28
Framework Example

• Simulation Loop
• Update Obstacles
• Apply Planning Constraint Forces
• Enforce Problem Constraints
• Repeat Until Goal is Achieved

29
General Framework
Simulation Loop
30
General Framework
Robots, Obstacles, Goals

Simulation Loop
31
General Framework
Robots, Obstacles, Goals

Simulation Loop
C1
Constraints
C2
C3

32
General Framework
INPUT Robots, Obstacles, Goals

Simulation Loop
C1
Constraints
C2
C3

Constraint Force
Energy Function
33
General Framework
Robots, Obstacles, Goals

Simulation Loop
S1
C1
Constraint Solvers
Constraints
C2
S2
C3
S3


34
General Framework
Robots, Obstacles, Goals

Simulation Loop
S1
C1
Constraint Solvers
Constraints
C2
S2
C3
S3
Run Simulation


Planned Path
35
Types of Constraints

• Hard Constraints
• Soft Constraints

36
Hard Constraints

• Must be enforced throughout the entire simulation
  
• Solved using Gauss-Seidel Iteration
• Examples
• object non-penetration
• joint connectivity
• joint angle limits

37
Gauss-Seidel Iteration

• For each hard constraint we require an Instance
  Solver , Relax()
• After applying Relax(Ci) the residual of the
  constraint Ci, Res(Ci) 0

38
Gauss-Seidel Iteration

• let S be the state of the simulation
• Repeat
• for each hard constraint Ci
• S ? Relax(Ci)
• until ?Res(Ci) 0

39
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
Joint Constraints

• Simple Atomic Constraints

41
Joint Constraints

• Simple Atomic Constraints
• point distance constraint

d
p2
p1
42
Joint Constraints

• Simple Atomic Constraints
• point distance constraint
• point planar angle constraint

d
p2
p1
43
Residuals

• Simple Atomic Constraints
• point distance constraint
• point planar angle constraint

44
Joint Constraints

• Combine atomic constraints to form joints

Example1 A Ball Joint
45
Joint Constraints

• Combine atomic constraints to form joints

Example 2 A Revolute Joint
46
Soft Constraints

• Encourage planning behavior
• Solved using penalty forces
• Examples
• goal seeking
• obstacle avoidance
• estimated path following

47
Voronoi Diagrams

• Partition space into regions by closest
  primitive
• Discretized version can be computed quickly using
  graphics hardware Hoff et al. 1999

48
Voronoi Diagrams

• Provide key planning constraints
• Global Estimated Paths
• Local Obstacle Avoidance

49
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
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
Obstacle Avoidance Example
R1 must be farther from R2 than a specified
threshold distance
52
Obstacle Avoidance Example
Localize computation using bounding boxes
53
Obstacle Avoidance Example
Compute distance field of R2 in local region
54
Obstacle Avoidance Example
Apply forces at sample points on R1
55
Obstacle Avoidance Example
Resultant force pushes R1 away from R2
56
Obstacle Avoidance

• Distance Field can be recomputed every frame
• Applicable to deformable robots obstacles
  whose shape changes every frame

57
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
Maintainability Study
Goal
Start

• Scene
• static environment with 2 moving robots
• 20,000 polygons
• Constraints
• Non-Penetration, Estimated Path, Obstacle
  Avoidance

59
Maintainability Study

• Performance
• Average Time Step 0.093 seconds
• Total Time 67 seconds
• The main bottleneck is the distance field
  calculation
• Video

60
Automotive Painting
Start
Goal

• Scene
• static environment and 6 linked moving objects
  (robot arm)
• 25,000 polygons
• Constraints
• Non-Penetration, Estimated Path, Obstacle
  Avoidance, 40 atomic joint constraints

61
Automotive Painting

• Performance
• Average Time Step 0.038 seconds
• Total Time 18 seconds
• Video

62
Assembly Line Planning
Start
Goal

• 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

63
Assembly Line Planning

• Performance
• Average Time Step 0.0085 seconds
• Total Time 16 seconds
• Video

64
Conclusion

• Planner
• Dynamic scenes using local constraints
• Global planning, using estimated path constraints
• Articulated objects represented using constraints

65
Conclusion

• Framework
• Static and dynamic environments
• General relationships between objects
• Extensible to many application areas

66
Future Work

• More Challenging Scenes
• Narrow Passages
• Many Dynamic Obstacles
• Deformable Objects

67
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