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

2. 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

3. 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

4. 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.

5. 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

6. 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

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

8. 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

9. 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

10. 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

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

12. 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]

13. 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

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

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

16. 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

17. First Graph Cut Algorithm
Only subdivide along L

18. Finding a Path by ACD
Here is the summary of

19. ACD for Path Non-existence
Initial
Goal
C-space

20. 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.

21. 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

22. 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

23. 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

24. 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

25. 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 …

26. 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

27. ‘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

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

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

30. 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?

31. 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

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

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

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

35. Algorithm Gf
Gsf G

36. Results of Hybrid Planning

37. Results of Hybrid Planning

38. 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.

39. 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

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

41. 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

42. 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