1. Chapter 5: Path Planning Hadi Moradi

2. Motivation Need to choose a path for the end effector that avoids collisions and singularities Collisions are easy to define in the workspace, but need to be mapped into the configuration space for convenience

3. Workspace v. configuration space Workspace: volume swept out by the end effector (in inertial frame) Configuration: location of all points on a robotic manipulator Configuration space:

4. Obstacles Discrete obstacles are denoted O i (in the workspace) Denote the robot as A (q) at configuration q The configuration space obstacle, QO, is defined as: The free configuration space is the space of all collision-free configurations:

5. Motion Planning for a Point Robot free space s g free path

6. Problem semi-free path

7. Types of Path Constraints  Local constraints: lie in free space  Differential constraints: have bounded curvature  Global constraints: have minimal length

8. Motion-Planning Framework Continuous representation Discretization Graph searching (blind, best-first, A*)

9. Example: Visibility Graph (A Roadmap Method)  Visibility graph Introduced in the Shakey project at SRI in the late 60s. Can produce shortest paths in 2-D configuration spaces g s

10. Example: Voronoi Diagram (A Roadmap Method)  Voronoi diagram Introduced by Computational Geometry researchers. Generate paths that maximizes clearance. O(n log n) time O(n) space

11. Cell-Decomposition Methods Two classes of methods:  Exact cell decomposition  Approximate cell decomposition F is represented by a collection of non-overlapping cells whose union is contained in F Examples: quadtree, octree, 2 n -tree

12. Approximate Cell Decomposition: Quad Tree

13. Octree Decomposition (3D environment)

14. Potential Field Methods Goal Robot  Approach initially proposed for real-time collision avoidance [Khatib, 86]. Hundreds of papers published on it.

15. Attractive and Repulsive fields

16. Potential Fields

17. Local-Minimum Issue  Perform best-first search (possibility of combining with approximate cell decomposition)  Alternate descents and random walks  Use local-minimum-free potential (navigation function)

18. Ex: 2D Cartesian manipulator The configuration space is R 2 Consider only one object in the workspace –End effector and obstacle are convex polygons What is the configuration space obstacle?

19. Ex: 2D Cartesian manipulator The nice thing about this example is that the workspace and the configuration space are identical

20. Ex: planar two-link manipulator What is the configuration space obstacle for a two-link manipulator

21. Motivation Geometric complexity Space dimensionality

22. Path planning overview Want to find a path from an initial position to a final position

23. Potential fields To develop the mapping, we incrementally explore Q free Consider the manipulator (statically) as a point in the configuration space The manipulator is subject to a potential field –Attractive in the case of the goal configuration –Repulsive in the case of an obstacle

24. Gradient descent In order to find minima of U, take the negative gradient:

25. The attractive field We define a potential field that attracts each of the n DH coordinate frames from the initial position to the goal position

26. The attractive field Simple potential field, conic well potential

27. The attractive field Instead we use a continually differentiable function: parabolic well potential –Field grows quadratically with the distance from the goal configuration

28. Hybrid attractive field Combine the conic well potential and parabolic well potential fields –If the i th frame is close to the workspace goal, use the parabolic well –If the i th frame is far from the workspace goal, use the conic well The distance d defines the distance from the goal that causes a transition from a conic to parabolic potential Since this is continuous everywhere, the workspace force is defined everywhere

29. Hybrid attractive field Taking the gradient gives the workspace attractive force

30. Ex: planar two link manipulator For the 2-link arm shown below, assume that both links have length 1

31. The repulsive field Prevent collisions by creating a repulsive force in the workspace –Again, create forces that act on the origins of the n DH coordinate frames These forces should: –Repel the robot from obstacles –Do nothing of the robot is far away from obstacles

32. The repulsive field Therefore, the workspace repulsive force is: To evaluate this, consider the distance function  (o i (q)) as  (x) where x is a three dimensional vector:

33. The repulsive field So we can write this force as:

34. Ex: planar two link manipulator Consider a convex obstacle close to o 2 –Obstacle is outside the distance of influence for o 1 –Again, the lengths are both 1 –Let b be the point on the obstacle closest to o 2 b = [2 0.5] T  (o 2 (q s )) = 0.5 –Let  0 = 1 (no influence on o 1 ) –The initial repulsive force on o 2 is:

35. Other considerations 1.what happens if either there are multiple objects, or an object is not convex?

36. Other considerations 2.what if the obstacle is closest to another part of a link (i.e. not the origin of the DH frame)?

37. The relation between workspace forces and joint torques

38. Ex: two-link planar manipulator Consider the previous examples with an obstacle exerting a repulsive force on o 2 Find the attractive and repulsive forces on o 1 and o 2 Initial and goal configurations Obstacle location

39. Ex: two-link planar manipulator To determine the joint torques, take the transpose of the Jacobians at the initial configuration

40. Composing workspace forces The total joint torques acting on a manipulator is the sum of the torques from all attractive and repulsive potentials:

41. Ex: two-link planar manipulator Consider again the two-link manipulator with a goal position and an obstacle near o 2 The total joint torque, due to these two potential fields is: Initial and goal configurationsObstacle location

42. Gradient descent Path Planning Algorithm 1.First, determine your initial configuration 2.Second, given a desired point in the workspace, calculate the final configuration using the inverse kinematics –Use this to create an attractive potential field 3.Locate obstacles in the workspace –Create a repulsive potential field 4.Sum the joint torques in the configuration space 5.Use gradient descent to reach your target configuration

43. Local minima In the absence of obstacles, the gradient descent will always converge to the global minimum (q f ) With obstacles, by proper choice of  i, this will always converge to some minima

44. Local minima Instead we modify the gradient descent algorithm to add a random excitation in case we are stuck in a local minima We are stuck in a local minima if successive iterations result in minimal changes in the configuration If so, perform a random walk to get out The random walk is defined by adding a uniformly distributed variable to each joint parameter

45. Next class… Applications to numerically solving for the inverse kinematics Probabilistic methods