# Complete Motion Planning

## Presentation on theme: "Complete Motion Planning"— Presentation transcript:

Complete Motion Planning
Liang-Jun Zhang Robotics, Comp Oct 26, 2006

Outline Motivation/Challenge Approaches Exact Motion Planning
Approximation Cell Decomposition Hybrid Planner 1. Motivation 1.1 What is complete motion planning, why we need complete motion planning 2. Challenge 2.1 DOF Complexity: Any path VS all paths 4. Methods 4.1 Compute the complete representation of the C-space 4.1.1 4.1.2 Star-shaped roadmap 4.2. Approximation Cell Decomposition The decomposition c-space into lots cells The adjacent graph How to make sure that no path, animation First graph cut How to label each cells. The big picture for collision detection of a set of configurations. 4.3. Hybrid Planner The ACD method is not. Could we combine it with 5. Reference Complete Motion Planning Star-shaped roadmap Complexity of algorithm ACD methods

Motion Planning To find a path Goal To report no path Goal Robot Robot
72 DOF Courtesy of P. Isto and M. Saha, 2006 Goal Initial Obstacle To report no path Robot Goal Robot The motion planning is to find a collision free path for the robot between initial and goal configurations. The following figure shows a simple example. Motion planning has been extensively studied. The successful approach, such as sampling based methods can deal with complex robot. For example, the degree of freedom of the robot can be up to a few hundreds. Sometimes, no path exists in the input problem. The right figure shows such example. The robot is blocked by the obstacle. At this time, we desire the planning approach can report path non-existence information. However, less attention has been paid to this side of motion planning. % due to complexity to decide the path non-existence, % However, to report path non-existence is more difficult. Even for this simple example, the sampling based approach % doesn’t work correctly. Initial Obstacle

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.

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

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

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

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

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

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

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

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]

Approximation Cell Decomposition
Configuration Space Full cell Empty cell Mixed cell Mixed Uncertain full mixed 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

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

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

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

First Graph Cut Algorithm
Only subdivide along L

Finding a Path by ACD Here is the summary of

ACD for Path Non-existence
Initial Goal C-space

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.

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

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

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

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

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 …

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

‘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

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

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

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?

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

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

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

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

Algorithm Gf Gsf G

Results of Hybrid Planning

Results of Hybrid Planning

Results of Hybrid Planning
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.

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

Future Work Complete motion planning for 6DOF rigid robots
More accurate PDg computation Efficient C-Obstacle representation and computation Extend for articulated robots

Reference: Exact Motion Planning
J. Canny. The Complexity of Robot Motion Planning. ACM Doctoral Dissertation Award. MIT Press, 1988. F. Avnaim and J.-D. Boissonnat. Practical exact motion planning of a class of robots with three degrees of freedom. In Proc. of Canadian Conference on Computational Geometry, page 19, 1989. J. T. Schwartz and M. Sharir. On the piano movers probelem ii, general techniques for computing topological properties of real algebraic manifolds. Advances of Applied Maths, 4:298–351, 1983. Gokul Varadhan, Dinesh Manocha, Star-shaped Roadmaps - A Deterministic Sampling Approach for Complete Motion Planning, Robotics: Science and Systems 2006

Reference: Approximation Cell Decomposition
T. Lozano-P´erez and M. Wesley. An algorithm for planning collisionfree paths among polyhedral obstacles. Comm. ACM, 22(10):560–570, 1979. R. A. Brooks and T. Lozano-P´erez. A subdivision algorithm in configuration space for findpath with rotation. IEEE Trans. Syst, SMC-15:224–233, 1985. D. Zhu and J. Latombe. Constraint reformulation in a hierarchical path planner. Proceedings of International Conference on Robotics and Automation, pages 1918–1923, 1990. L. Zhang, Y. Kim, and D. Manocha. A simple path non-existence algorithm using c-obstacle query. In Proc. of WAFR, 2006. L. Zhang, Y.J. Kim, and D. Manocha, A Hybrid Approach for Complete Motion Planning, UNC-CS Tech Report