1. Motion Planning & Robot Planning
Prof.: S. Shiry
Mohsen gandomkar
M.Sc Artificial Intelligence
Department of Computer Eng. and IT
Amirkabir Univ. of Technology
(Tehran Polytechnic)

2. What is Motion Planning?
Motion planning is aimed at providing robots with the capability of deciding automatically which motions to execute in order to achieve their tasks without colliding with other objects in their work space

3. Basic Definition Obstacles Already occupied spaces of the world
In other words, robots can’t go there
Free Space
Unoccupied space within the world
Robots “might” be able to go here
To determine where a robot can go, we need to discuss what a Configuration Space is

4. For a point robot moving in 2-D plane, C-space is
The Configuration Space
Configuration Space of A is the space (C) of all possible configurations of A. C
Cfree
qgoal
Cobs
qstart
For a point robot moving in 2-D plane, C-space is

5. For a point robot moving in 3-D, the C-space is
The Configuration Space C y
Cfree
qgoal Z
Cobs
qstart x
For a point robot moving in 3-D, the C-space is
What is the difference between Euclidean space and C-space?

6. The Configuration Space
How to create it
First abstract the robot as a point object. Then, enlarge the obstacles to account for the robot’s footprint and degrees of freedom
In our example, the robot was circular, so we simply enlarged our obstacles by the robot’s radius (note the curved vertices)

7. Example of a World (and Robot)
Obstacles
Free Space
Robot
x,y

8. Configuration Space: Accommodate Robot Size
Obstacles
Free Space
Robot
(treat as point object)
x,y

9. Motion Planning
Basic problem: Collision-free path planning for one rigid or articulated object (the “robot”) among static obstacles.
Inputs
geometric descriptions of the obstacles and the robot
kinematic and dynamic properties of the robot
initial and goal positions (configurations) of the robot
Output
Continuous sequence of collision-free configurations connecting the initial and goal configurations.

10. Algorithmic Approaches
Complete Algorithms
Probabilistic Algorithms
Heuristic Algorithms

11. Complete Algorithms
Guaranteed to find a free path between two give configurations when exists and report failure otherwise
Deal with connectivity of free space by capturing it on a graph.
Cell Decomposition - partition of free space
Roadmap Technique - network of curves

12. Probabilistic Algorithms
Trade-off exactness against running time
Don’t guarantee a solution but if exists very likely to find it relatively quickly
Example: Probabilistic Roadmap Algorithm
Experimental results show that computation takes less than a second

13. Heuristic Algorithms
Many work well in practice but offer no performance guarantee
Deal with a grid on configuration space
Example 1 : Potential Field
Example 2 : Approximate Cell Decomposition

14. Previous Approaches

15. Visibility Graphs

16. Voronoi Diagrams

17. Exact Cell Decomposition

18. Approximate Cell Decomposition

19. Potential Fields

20. Probabilistic Roadmaps Method

21. Problems before PRMs Hard to plan for many dof robots
Computation complexity for high-dimensional configuration spaces would grow exponentially
Potential fields run into local minima
Complete, general purpose algorithms are at best exponential and have not been implemented

22. Difficulty with classic approaches
Running time increases exponentially with the dimension of the configuration space.
For a d-dimension grid with 10 grid points on each dimension, how many grid cells are there?
Several variants of the path planning problem have been proven to be PSPACE-hard.
10d

23. Probabilistic Roadmap (PRM): multiple queries
local path
free space
milestone
[Kavraki, Svetska, Latombe,Overmars, 96]

24. Probabilistic Roadmap (PRM): single query

25. Multiple-Query PRM

26. Classic multiple-query PRM
Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces, L. Kavraki et al., 1996.

27. Assumptions Static obstacles
Many queries to be processed in the same environment
Examples
Navigation in static virtual environments
Robot manipulator arm in a workcell

28. Enter PRMs PRMs use fast collision checking techniques
PRMs avoid computing an explicit representation of the configuration space
Two Phases
A Learning Phase
A Query Phase

29. The Learning Phase
Construct a probabilistic roadmap by generating random free configurations of the robot and connecting them using a simple, but very fast motion planer, also know as a local planner
Store as a graph whose nodes are the configurations and whose edges are the paths computed by the local planner

30. PRM - Learning Phase
C-obstacle
Free space

31. PRM - Learning Phase
C-obstacle
Free space

32. PRM - Learning Phase
C-obstacle
Free space
milestones

33. PRM - Learning Phase
C-obstacle
Free space
milestones

34. The Query Phase
Find a path from the start and goal configurations to two nodes of the roadmap
Search the graph to find a sequence of edges connecting those nodes in the roadmap
Concatenating the successive segments gives a feasible path for the robot

35. Two geometric primitives in configuration space
CLEAR(q) Is configuration q collision free or not?
LINK(q, q’) Is the straight-line path between q and q’ collision-free?

36. Uniform sampling Input: geometry of the moving object & obstacles
Output: roadmap G = (V, E)
1: V   and E  .
2: repeat
3: q  a configuration sampled uniformly at random from C.
4: if CLEAR(q)then
5: Add q to V.
6: Nq  a set of nodes in V that are close to q.
6: for each q’ Nq, in order of increasing d(q,q’)
7: if LINK(q’,q)then
8: Add an edge between q and q’ to E.

37. Difficulty
Many small connected components

38. Resampling (expansion)
Failure rate
Weight
Resampling probability

39. Resampling (expansion)

40. Query processing Connect qinit and qgoal to the roadmap
Start at qinit and qgoal, perform a random walk, and try to connect with one of the milestones nearby
Try multiple times

41. Error If a path is returned, the answer is always correct.
If no path is found, the answer may or may not be correct. We hope it is correct with high probability.

42. Why does it work? Intuition
A small number of milestones almost “cover” the entire configuration space.

43. Smoothing the path

44. Smoothing the path

45. Single-Query PRM

46. Lazy PRM
Path Planning Using Lazy PRM, R. Bohlin & L. Kavraki, 2000.

47. Precomputation: roadmap construction
Nodes
Randomly chosen configurations, which may or may not be collision-free
No call to CLEAR
Edges
an edge between two nodes if the corresponding configurations are close according to a suitable metric
no call to LINK

48. Query processing: overview
Find a shortest path in the roadmap
Check whether the nodes and edges in the path are collision.
If yes, then done. Otherwise, remove the nodes or edges in violation. Go to (1).
We either find a collision-free path, or exhaust all paths in the roadmap and declare failure.

49. Query processing: details
Find the shortest path in the roadmap
A* algorithm
Dijkstra’s algorithm
Check whether nodes and edges are collisions free
CLEAR(q)
LINK(q0, q1)

50. Node enhancement
Select nodes that close the boundary of F

51. Bug algorithms

52. Bug algorithms Assumptions: Point robot
Contact sensor (Bug1,Bug2) or finite range sensor (Tangent Bug)
Bounded environment
Robot position is perfectly known
Robot can measure the distance between two points

53. Bug algorithms Algorithm consists of two behaviors:
1. Motion to goal – move toward the goal
Bug1: move along the line that connects an “initial” point to the goal until you reach the goal or an obstacle (hit point).
Bug2: move along the line that connects the start point to the goal until you reach the goal or an obstacle (hit point).

54. Bug algorithms 2. Boundary following – obstacle handeling
Bug1: circumnavigate the entire perimeter of the obstacle, find the closest point to the goal on the perimeter (leave point), move to that point .
Bug2: circumnavigate the obstacle until you reach a new point on the line connecting start and goal, that is closer to the goal (leave point).

55. Bug1 - example q2L qstart qgoal q2H q1L q1H Motion to goal
Boundary following
Shortest path to goal

56. Bug2 - example q2L qstart qgoal q1L q2H q1H Motion to goal
Boundary following
Line connecting start and goal

57. head-to-head comparison
What are worlds in which Bug 2 does better than Bug 1 (and vice versa) ?
Bug 2 beats Bug 1
Bug 1 beats Bug 2
Start

58. head-to-head comparison
What are worlds in which Bug 2 does better than Bug 1 (and vice versa) ?
Bug 2 beats Bug 1
Bug 1 beats Bug 2
“zipper world”
Start

59. Problem
Bug 2 beats Bug 1
Bug 1 beats Bug 2
“zipper world”

60. Problem Adjusted bug algorithm Bug M1
use Bug2 until the robot finds itself on the S-line farther from the goal than it started
if it does, revert to to Bug1 for that obstacle
Adjusted bug algorithm

61. Problem Adjusted bug algorithm Bug M1
use Bug2 until the robot finds itself on the S-line farther from the goal than it started
if it does, revert to to Bug1 for that obstacle
Adjusted bug algorithm
Bug M1

62. Bug1 vs. Bug2 Bug1 Bug2 Exhaustive search Optimal leave point
Performs better with complex obstacles
Path length :
n = # of obstacles
Pi = perimeter of obstacle i
Bug2
Opportunistic (greedy) search
Performs better with simple obstacles
Path length :
ni = # of times the start-goal line intersects obstacle i

63. Finite range sensor
Intervals of continuity

64. Tangent Bug algorithm Improvement to the Bug2 algorithm Assumptions:
All assumptions of Bug1/Bug2 except for contact sensor.
Finite range sensor with 360◦ infinite orientation resolution.

65. Tangent Bug algorithm Like Bug1/Bug2, iterates between two behaviors:
motion to goal – consists of two parts:
Move in a straight line towards the goal until you sense an obstacle directly between you and the goal
Move toward an intermediate point* Oj according to some heuristic distance** until you reach the goal or until you reach a local minimum Mi in which case, switch to boundary following
* Oj‘s are end points of an interval of continuity
** For example d(x, Oj)+ d(Oj,goal)

66. Tangent Bug algorithm Motion to goal t = 1 t = 2 t = 3 o1 o2 t = 4 o1

67. boundary following – define two distances:
Tangent Bug algorithm
boundary following – define two distances:
dfollowing – The shortest distance between the sensed boundary and the goal
dreach – The distance between the point on the boundary that has a line of sight to the goal, and the goal
continue moving around the obstacle in the same direction until dreach < dfollowing then switch to motion to goal

68. Tangent Bug algorithm
Boundary following
Motion to goal M
goal

69. Tangent Bug - example
qgoal
qstart
Motion to goal
Boundary following

70. Bug algorithms Simple and intuitive Straightforward to implement
Success guaranteed (when possible)
Assumes perfect positioning and sensing
Sensor based planning – has to be incremental and reactive

71. Multi-Robot Planning

72. Multi-Robot Planning Examples

73. Multi-Robot Planning
An initial and a goal configuration are given as input for each robot
Result is a coordinated path between the two configurations
A coordinated path is one that indicates the configuration of every robot at each instant
Collisions must be avoided between each pair of robot and obstacles, and between each pair of robots

74. Centralized Planning
Paths for all robots are planned simultaneously by searching the c-space of the multi-arm robot
Collisions between robots are self-collisions of the multi-arm robot
For spot-welding example, 6 robots each with 6 dofs, so C will have 36-D

75. Centralized Planning Advantages Disadvantages
Complete – guaranteed to find a solution if one exists (if the underlying planner is complete)
Disadvantages
Potentially expensive – typically requires searching high-dimensional spaces
Requires knowledge of goals and states of all robots

76. Decoupled Planning
First Phase - a collision-free path ti is generated for each robot considering only obstacles (ignoring other robots) in its space

77. Decoupled Planning
Second Phase (Velocity Tuning) – coordination of the robots’ velocities along their pre-generated paths to prevent collisions between robots. Two coordination methods discussed
Pairwise Coordination
Global Coordination
Each robot is restricted to motion in its pre-generated path although it may stop, retreat or change velocity to allow coordination with other robots

78. Decoupled Planning with Pairwise Coordination
The paths t1 and t2 of the first two robots are coordinated in their 2-dimensional coordination space
Results in a collision-free coordinated patht1,2
Done by using planning a path between (0,0) and (1,1)

79. Decoupled Planning with Pairwise Coordination
The process is repeated for paths t1,2 and t3 resulting in a coordinated path t1,2,3
Eventually a collision-free coordinate path t1,2,…,m is generated that defines a valid coordination of all m robots

80. Decoupled Planning with Global Coordination
The paths of all m robots are coordinated in an m-dimensional coordination space
Results in a collision-free path t1,2,….m
Done by planning a path from (0,0,0,…) to (1,1,1,…)

81. Decoupled Planning Advantages Disadvantages
Less expensive than centralized planning because lower dimensional spaces are searched
Disadvantages
Incomplete : Failures usually occur in the second phase as it might not be possible to coordinate the paths generated in the first phase without collision between robots

82. Decoupled Planning Failure Example
Initial and goal configurations

83. Decoupled Planning Failure Example
Likely path generation in 1st phase

84. Decoupled Planning Failure Example
Path coordination fails in second phase

85. Implemented Planners C-SBL – Centralized Planning
DG-SBL – Decoupled Planning with Global Coordination
DP-SBL – Decoupled Planning with Pairwise Coordination
Experiments conducted with groups of 2, 4 and 6 robots on 3 separate sets of initial/goal configurations

86. PRM Path Planner: Sampling Strategy
SBL Planner
Single-query
Bi-directional
Lazy collision-checking

87. Problem I – Initial and goal configurations

88. Problem II – Initial and goal configurations

89. Problem III – Initial and goal configurations

90. Experimental Results T = average running time (seconds)
DG-SBL and DP-SBL - 20 runs per experiment
C-SBL – 100 runs per experiment
F = number of failures
Maximum of 50,000 milestones allowed per call to SBL

91. Experimental Results Centralized planning had no failures
At least one failure suffered in each experiment with decoupled planning
Failure rate increased as problems became more complex

92. Experimental Results
pairwise coordination more unreliable than global coordination
Failure always occurred in the 2nd stage during path coordination, a result of wrong path choices made in the 1st stage

93. Experimental Results
Similar running times for both planners in most experiments
However, centralized planning required a lot more time than decoupled planning in 3rd problem with 6 robots

94. Conclusions
Reliability – Decoupled planning can be quite unreliable particularly in tight robot coordination. Centralized planning appears to have perfect reliability.
Planning Time – Using SBL, there is not a huge difference between the two methods

95. Conclusions Contd.
Results invalidate the assumptions that loss of incompleteness with decoupled planning is fairly insignificant and can be ignored in practice.
SBL makes usage of centralized planning for multi-robot systems practical.
But centralized planning still requires knowledge of all robot states, which may be impossible in some settings.

96. Sokoban Objective of Robot:
To push boxes into their storage locations without getting himself or boxes stuck.
Rules: Cannot pull, can push only one box at a time

97. Sokoban

98. Sample Sokoban Game