Robot Lab: Robot Path Planning William Regli Department of Computer Science (and Departments of ECE and MEM) Drexel University Slide 1

Introduction to Motion Planning Applications Overview of the Problem Basics – Planning for Point Robot Visibility Graphs Roadmap Cell Decomposition Potential Field Slide 2

Goals Compute motion strategies, e.g., Achieve high-level goals, e.g., Geometric paths Time-parameterized trajectories Sequence of sensor-based motion commands Achieve high-level goals, e.g., Go to the door and do not collide with obstacles Assemble/disassemble the engine Build a map of the hallway Find and track the target (an intruder, a missing pet, etc.) Slide 3

Fundamental Question Are two given points connected by a path? Slide 4

Basic Problem Problem statement: Compute a collision-free path for a rigid or articulated moving object among static obstacles. Input Geometry of a moving object (a robot, a digital actor, or a molecule) and obstacles How does the robot move? Kinematics of the robot (degrees of freedom) Initial and goal robot configurations (positions & orientations) Output Continuous sequence of collision-free robot configurations connecting the initial and goal configurations Slide 5

Example: Rigid Objects Slide 6

Example: Articulated Robot Slide 7

Is it easy? Slide 8

Hardness Results Several variants of the path planning problem have been proven to be PSPACE-hard. A complete algorithm may take exponential time. A complete algorithm finds a path if one exists and reports no path exists otherwise. Examples Planar linkages [Hopcroft et al., 1984] Multiple rectangles [Hopcroft et al., 1984] Slide 9

Tool: Configuration Space Difficulty Number of degrees of freedom (dimension of configuration space) Geometric complexity Slide 10

Extensions of the Basic Problem More complex robots Multiple robots Movable objects Nonholonomic & dynamic constraints Physical models and deformable objects Sensorless motions (exploiting task mechanics) Uncertainty in control Slide 11

Extensions of the Basic Problem More complex environments Moving obstacles Uncertainty in sensing More complex objectives Optimal motion planning Integration of planning and control Assembly planning Sensing the environment Model building Target finding, tracking Slide 12

Practical Algorithms A complete motion planner always returns a solution when one exists and indicates that no such solution exists otherwise. Most motion planning problems are hard, meaning that complete planners take exponential time in the number of degrees of freedom, moving objects, etc. Slide 13

Practical Algorithms Theoretical algorithms strive for completeness and low worst-case complexity Difficult to implement Not robust Heuristic algorithms strive for efficiency in commonly encountered situations. No performance guarantee Practical algorithms with performance guarantees Weaker forms of completeness Simplifying assumptions on the space: “exponential time” algorithms that work in practice Slide 14

Problem Formulation for Point Robot Input Robot represented as a point in the plane Obstacles represented as polygons Initial and goal positions Output A collision-free path between the initial and goal positions Slide 15

Framework Slide 16

Visibility Graph Method Observation: If there is a collision-free path between two points, then there is a polygonal path that bends only at the obstacles vertices. Why? Any collision-free path can be transformed into a polygonal path that bends only at the obstacle vertices. A polygonal path is a piecewise linear curve. Slide 17

Visibility Graph A visibility graphis a graph such that Nodes: qinit, qgoal, or an obstacle vertex. Edges: An edge exists between nodes u and v if the line segment between u and v is an obstacle edge or it does not intersect the obstacles. Slide 18

A Simple Algorithm for Building Visibility Graphs Slide 19

Computational Efficiency Simple algorithm O(n3) time More efficient algorithms Rotational sweep O(n2log n) time Optimal algorithm O(n2) time Output sensitive algorithms O(n2) space Slide 20

Framework Slide 21

Breadth-First Search Slide 22

Breadth-First Search Slide 23

Breadth-First Search Slide 24

Breadth-First Search Slide 25

Breadth-First Search Slide 26

Breadth-First Search Slide 27

Breadth-First Search Slide 28

Breadth-First Search Slide 29

Breadth-First Search Slide 30

Breadth-First Search Slide 31

Other Search Algorithms Depth-First Search Best-First Search, A* Slide 32

Framework Slide 33

Summary Discretize the space by constructing visibility graph Search the visibility graph with breadth-first search Q: How to perform the intersection test? Slide 34

Summary Represent the connectivity of the configuration space in the visibility graph Running time O(n3) Compute the visibility graph Search the graph An optimal O(n2) time algorithm exists. Space O(n2) Can we do better? Slide 35

Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function Slide 36

Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function Slide 37

Roadmap Visibility graph Shakey Project, SRI [Nilsson, 1969] Voronoi Diagram Introduced by computational geometry researchers. Generate paths that maximizes clearance. Applicable mostly to 2-D configuration spaces. Slide 38

Voronoi Diagram Space O(n) Run time O(n log n) Slide 39

Other Roadmap Methods Silhouette First complete general method that applies to spaces of any dimensions and is singly exponential in the number of dimensions [Canny 1987] Probabilistic roadmaps Slide 40

Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function Slide 41

Cell-decomposition Methods Exact cell decomposition The free space F is represented by a collection of non-overlapping simple cells whose union is exactly F Examples of cells: trapezoids, triangles Slide 42

Trapezoidal Decomposition Slide 43

Computational Efficiency Running time O(n log n) by planar sweep Space O(n) Mostly for 2-D configuration spaces Slide 44

Adjacency Graph Nodes: cells Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent. Slide 45

Summary Discretize the space by constructing an adjacency graph of the cells Search the adjacency graph Slide 46

Cell-decomposition Methods Exact cell decomposition Approximate cell decomposition F is represented by a collection of non-overlapping cells whose union is contained in F. Cells usually have simple, regular shapes, e.g., rectangles, squares. Facilitate hierarchical space decomposition Slide 47

Quadtree Decomposition Slide 48

Octree Decomposition Slide 49

Algorithm Outline Slide 50

Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function Slide 51

Potential Fields Initially proposed for real-time collision avoidance [Khatib 1986]. Hundreds of papers published. A potential field is a scalar function over the free space. To navigate, the robot applies a force proportional to the negated gradient of the potential field. A navigation function is an ideal potential field that has global minimum at the goal has no local minima grows to infinity near obstacles is smooth Slide 52

Attractive & Repulsive Fields Slide 53

How Does It Work? Slide 54

Algorithm Outline Place a regular grid G over the configuration space Compute the potential field over G Search G using a best-first algorithm with potential field as the heuristic function Slide 55

Local Minima What can we do? Escape from local minima by taking random walks Build an ideal potential field – navigation function – that does not have local minima Slide 56

Question Can such an ideal potential field be constructed efficiently in general? Slide 57

Completeness A complete motion planner always returns a solution when one exists and indicates that no such solution exists otherwise. Is the visibility graph algorithm complete? Yes. How about the exact cell decomposition algorithm and the potential field algorithm? Slide 58

Why Complete Motion Planning? Probabilistic roadmap motion planning Efficient Work for complex problems with many DOF Difficult for narrow passages May not terminate when no path exists Complete motion planning Always terminate Not efficient Not robust even for low DOF When the input problem does not have a path, no matter how many samples the planner make, it can never end. Slide 59

Path Non-existence Problem Initial Robot Obstacle Obstacle Goal Here is a slightly more complex example. We have 2 gear shaped obstacles in 2D plane. We need to move the gear-shaped robot from left to right through these two obstacles. The figure shows that the robot can be placed between these two obstacles. But can you quickly identify path non-existence for this problem? % But it’s not intuitive to tell whether there is a collision-free path or no path exists for this example. % In some sense, to claim the path non-existence is more difficulty than to find a collision free path. % And demands more computations Slide 60

Main Challenge Exponential complexity: nDOF Degree of freedom: DOF Geometric complexity: n More difficult than finding a path To check all possible paths Initial Robot Obstacle Here is a slightly more complex example. We have 2 gear shaped obstacles in 2D plane. We need to move the gear-shaped robot from left to right through these two obstacles. The figure shows that the robot can be placed between these two obstacles. But can you quickly identify path non-existence for this problem? % But it’s not intuitive to tell whether there is a collision-free path or no path exists for this example. % In some sense, to claim the path non-existence is more difficulty than to find a collision free path. % And demands more computations Goal Slide 61

Approaches Exact Motion Planning Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid planner Slide 62

Configuration Space: 2D Translation Workspace Configuration Space Goal Obstacle C-obstacle Free Robot The figure shows a triangle shaped robot translating in a 2D environment. Each position of the robot maps to a point in the configuration space. If we add an obstacle, it maps to a region in the configuration space. This region is called the configuration space obstacle or C-obstacle. y x Start Slide 63

Configuration Space Computation [Varadhan et al, ICRA 2006] 2 Translation + 1 Rotation 215 seconds Obstacle  y x The configuration space is three dimensional. I have shown the contact surfaces. These are close to 4000 surfaces. And these surfaces are non-linear and can have a maximum degree of 4. Robot Slide 64

Exact Motion Planning Approaches Not practical Exact cell decomposition [Schwartz et al. 83] Roadmap [Canny 88] Criticality based method [Latombe 99] Voronoi Diagram Star-shaped roadmap [Varadhan et al. 06] Not practical Due to free space computation Limit for special and simple objects Ladders, sphere, convex shapes 3DOF Slide 65

Approaches Exact Motion Planning Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid Planner Combing ACD and PRM Slide 66

Approximation Cell Decomposition (ACD) Not compute the free space exactly at once But compute it incrementally Relatively easy to implement [Lozano-Pérez 83] [Zhu et al. 91] [Latombe 91] [Zhang et al. 06] Slide 67

Approximation Cell Decomposition Configuration Space full mixed Full cell Empty cell Mixed cell Mixed Uncertain Now let us look at how C-obstacle query can be used for cell decomposition for path non-existence. The cell decomposition will subdivide the configuration space into cells. If a cell lies entirely in C-obstacle, the cell is labelled as full cell. If … Otherwise, … Here the full cells can be identified using C-obstacle query. empty Slide 68

Gf : Free Connectivity Graph G: Connectivity Graph Interpration Gf is a subgraph of G Slide 69

Gf : Free Connectivity Graph Finding a Path by ACD Gf : Free Connectivity Graph Initial Goal Slide 70

Described in Latombe’s book Finding a Path by ACD First Graph Cut Algorithm Guiding path in connectivity graph G Only subdivide along this path Update the graphs G and Gf L: Guiding Path Described in Latombe’s book Slide 71

First Graph Cut Algorithm Only subdivide along L Slide 72

Finding a Path by ACD Here is the summary of Slide 73

ACD for Path Non-existence Initial Goal C-space Slide 74

ACD for Path Non-existence Guiding Path Connectivity Graph The cell decomposition will build a connectivity graph for the complement of identified C-obstacle region. % for the complement of empty and mixed cells. The node in the graph is to represent each empty or mixed % cell. Two nodes are connected, if their corresponding cells are adjacent. Then it searches a guiding path connecting the cell containing the initial configuration, and the cell containing the goal configuration. The cell decomposition is only applied for the mixed on this path. % If all cells along this path are empty cells, a collision free path has been found. Otherwise, we have to % perform further subdivision. In order to avoid the substantial subdivision, a smarter way is to subdivide % the mixed cells along this path. That is the reason, this path as a guiding path. Slide 75

ACD for Path Non-existence Connectivity graph is not connected No path! After one level subdivision, we update the connectivity graph, and perform the graph search again. At this time, we find the graph is disjoint with respect to the initial and the goal configurations. Therefore, we can conclude no-path exists for this problem. From the above analysis, we can tell the C-obstacle query plays an important role to decide the path non-existence.??? However, most of previous work of C-obstacle query are either hard to implement or overly conservative. Sufficient condition for deciding path non-existence Slide 76

Two-gear Example Video no path! 3.356s Initial Cells in C-obstacle Roadmap in F The first example is two-gear, which we have seen before. We need to move the red gear from left corner to right corner. Now I will show a video for the running process of our algorithm. the configuration space, initial and goal configurations. The pink volume denotes the cells in C-obstacle identified by our C-obstacle query. A guiding search is searched. And the mixed cell is iterated. In the right figure, after a few iterations of cell decomposition, we could find no path exists since initial and goal configurations are separated by the C-obstacle region. Goal Slide 77

Cell Labeling Free Cell Query C-obstacle Cell Query full mixed Whether a cell completely lies in free space? C-obstacle Cell Query Whether a cell completely lies in C-obstacle? empty Slide 78

Free Cell Query A Collision Detection Problem Does the cell lie inside free space? Do robot and obstacle separate at all configurations? Robot Obstacle In order to design an efficient … let us analyze the problem of C-obstacle query. The C-obstacle query actually can be regarded as a class of collision detection problem. Suppose the query primitive is a cell. The C-obstacle query is to check whether the cell lies completely inside the c-obstacle. We know if the robot’s configuration is in C-obstacle, the robot is intersecting with the obstacle. To ensure a cell lies complete inside C-obstacle, we had to make sure at every configuration in the cell, the robot intersects with the obstacle. In another way, this means when the robot’s configuration varies within the cell, the robot can never escape from the obstacle. In fact, the later intuitive interpretation is very helpful to understand our C-obstacle query algorithm, which will be discussed in the next slide. ? Configuration space Workspace Slide 79

Clearance d Separation distance A well studied geometric problem Determine a volume in C-space which are completely free d Suppose two objects are disjoint, we can use the separation distance between them. This distance describes the clearance of the robot, here the robot. If the motion of the robot is less than the clearance, the robot can not intersect with the obstacle. The dual concept of clearance here is called forbiddance. When two objects intersects, we can measure the penetration depth between them. This depth describes the forbiddance, which means … Slide 80

C-obstacle Query Another Collision Detection Problem Does the cell lie inside C-obstacle? Do robot and obstacle intersect at all configurations? Robot ? Obstacle In order to design an efficient … let us analyze the problem of C-obstacle query. The C-obstacle query actually can be regarded as a class of collision detection problem. Suppose the query primitive is a cell. The C-obstacle query is to check whether the cell lies completely inside the c-obstacle. We know if the robot’s configuration is in C-obstacle, the robot is intersecting with the obstacle. To ensure a cell lies complete inside C-obstacle, we had to make sure at every configuration in the cell, the robot intersects with the obstacle. In another way, this means when the robot’s configuration varies within the cell, the robot can never escape from the obstacle. In fact, the later intuitive interpretation is very helpful to understand our C-obstacle query algorithm, which will be discussed in the next slide. Configuration space Workspace Slide 81

‘Forbiddance’ ‘Forbiddance’: dual to clearance Penetration Depth A geometric computation problem less investigated [Zhang et al. ACM SPM 2006] Suppose two objects are disjoint, we can use the separation distance between them. This distance describes the clearance of the robot, here the robot. If the motion of the robot is less than the clearance, the robot can not intersect with the obstacle. The dual concept of clearance here is called forbiddance. When two objects intersects, we can measure the penetration depth between them. This depth describes the forbiddance, which means … PD Slide 82

Limitation of ACD Combinatorial complexity of cell decomposition Limited for low DOF problem 3-DOF robots Slide 83

Approaches Exact Motion Planning Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid Planner Combing ACD and PRM Slide 84

Can we combine them together? Hybrid Planning Probabilistic roadmap motion planning + Efficient + Many DOFs Narrow passages Path non-existence Complete Motion Planning + Complete Not efficient Can we combine them together? Slide 85

Hybrid Approach for Complete Motion Planning Use Probabilistic Roadmap (PRM): Capture the connectivity for mixed cells Avoid substantial subdivision Use Approximation Cell Decomposition (ACD) Completeness Improve the sampling on narrow passages Slide 86

Gf : Free Connectivity Graph G: Connectivity Graph Interpration Gf is a subgraph of G Slide 87

Pseudo-free edges Initial Goal Pseudo free edge for two adjacent cells Slide 88

Pseudo-free Connectivity Graph: Gsf Gsf = Gf + Pseudo-edges Initial Goal Slide 89

Algorithm Gf Gsf G Slide 90

Results of Hybrid Planning Slide 91

Results of Hybrid Planning Slide 92

Results of Hybrid Planning 2.5 - 10 times speedup 3 DOF 4 DOF timing cells # Hybrid 34s 50K 16s 48K 102s 164K ACD 85s 168K ? Speedup 2.5 3.3 ≥10 For 4DOF, the ACD method could not terminate within 10 times of the timing for the hybrid planners. Slide 93

Summary Difficult for Exact Motion Planning ACD is more practical Due to the difficulty of free space configuration computation ACD is more practical Explore the free space incrementally Hybrid Planning Combine the completeness of ACD and efficiency of PRM Slide 94