1. CS 326 A: Motion Planning http://robotics.stanford.edu/~latombe/cs326/2004 Kinodynamic Planning

2. Underactuated Robots  Fewer controls than dimensions in configuration space  What is a degree of freedom: number of dimensions of C-space (global) or number of controls (local)?

3. How can m controls generate span a C-space with n>m dimensions? By exploiting mechanics properties: - Rolling-with-no-sliding contact (friction), e.g.,: car, bicycle, roller skate - Conservation of angular momentum: satellite robot, under-actuated robot, cat - Others: submarine, plane, object pushing Why is it useful? - Fewer actuators (less weight) - Design simplicity - Convenience (think about driving a car with 3 controls!)

4. Example: Car-Like Robot y x    Configuration space is 3-dimensional: q = ( x, y,  ) But control space is 2-dimensional: ( v,  ) with | v | = sqrt[(dx/dt) 2 +(dy/dt) 2 ] L dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0

5. Example: Car-Like Robot y x    L q = (x,y,  ) q’= dq/dt = (dx/dt,dy/dt,d  /dt) dx sin  – dy cos  = 0 is a particular form of f(q,q’)=0 A robot is nonholonomic if its motion is constrained by a non- integrable equation of the form f(q,q’) = 0 dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0

6. Example: Car-Like Robot y x    L Lower-bounded turning radius dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0

7. How Can This Work? Tangent Space/Velocity Space x y  (x,y,  )  y x    L dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0 (dx,dy,d  ) (dx,dy)

8. How Can This Work? Tangent Space/Velocity Space x y  (x,y,  )  y x    L dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0 (dx,dy,d  ) (dx,dy)

9. Nonholonomic Path Planning Approaches  Two-phase planning: (first paper)  Compute collision-free path ignoring nonholonomic constraints  Transform this path into a nonholonomic one  Efficient, but possible only if robot is “controllable”  Plus need to have “good” set of maneuvers  Direct planning: (second paper)  Build a tree of milestones until one is close enough to the goal (deterministic or randomized)  Robot need not be controllable  Works in high-dimensional c-spaces

10. Path Transform Holonomic path Nonholonomic path

11. Coverage of a Path by Cylinders x y  + q q’

12. Type 1 Maneuver  Allows sidewise motion CYL(x,y, ,  )  = 2  /cos  d = 2  (1/ cos  - 1)   

13. Type 2 Maneuver  Allows pure rotation

14. Combination

15. Coverage of a Path by Cylinders x y  + q q’

16. Combination

17. Path Examples

18. Drawbacks of Two-phase planning  Final path can be far from optimal  Not applicable to robots that are not locally controllable (e.g., car that can only move forward)

19. Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions X (  t) Y -X -Y

20. Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions For example: X: Going straight Y: Turning, angle  dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0

21. Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions For example: X (  t) Y -X -Y [X,Y] (  t 2 ) X: Going straight Y: Turning, angle 

22. Control-Based Sampling  Previous sampling technique: Pick each milestone in some region  Control-based sampling: 1.Pick control vector (at random or not) 2.Integrate equation of motion over short duration (picked at random or not) 3.The endpoint is the new milestone  Tree-structured roadmaps  Need for endgame regions

23. mbmb mgmg Control-Based Sampling endgame region

24. Example dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0 1. Select a milestone m 2. Pick v, , and  t 3. Integrate motion from m  new configuration

25. Computed Paths  max = 45 o,  min = 22.5 o Car That Can Only Turn Left  max = 45 o Tractor-trailer

26. Another Example Cooperative load carrying (Grasp Lab - U. Penn)

27. Nonholonomic vs. Dynamic Constraints  Nonholonomic constraint: q’ = f(q,u) where u is the control input (function of time), with dim(u) < dim(q) dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0 u = ( v,  )

28. Nonholonomic vs. Dynamic Constraints  Nonholonomic constraint: q’ = f(q,u) where u is the control input (function of time), with dim(u) < dim(q)  Dynamic constraint:  s = (q,q’), the state of the system  s’ = f(s,u) where u is the control input

29. General Dynamic Equation For an arbitrary mechanical linkage: u = M(q)q” + C(q,q’) + G(q) + F(q,q’) where: - M is the inertia matrix - C is the vector of centrifugal and Coriolis terms - G is the vector of gravity terms - F is the vector of friction terms + constraints on u

30. Nonholonomic vs. Dynamic Constraints  Nonholonomic constraint: q’ = f(q,u) where u is the control input (function of time), with dim(u) < dim(q)  Dynamic constraint:  s = (q,q’), the state of the system  s’ = f(s,u) where u is the control input  Similar techniques to handle nonholonomic and dynamic constraints (kinodynamic planning)

31. “Space” Robot (ARL Lab) air bearing gas tank air thrusters obstacles robot

32. Modeling of Robot x y  f q = (x,y) s = (q,q’) u = (f,  ) x” = (f/m) cos  y” = (f/m) sin  f  f max s’ = F(s,u)

33. mbmb mgmg PRM in State (x Time) Space The roadmap is a tree oriented along the time axis endgame region

34. Computed Path Mean planning time:.002 s Mean number of milestones: 22

35. Another Path Mean planning time:.27s Mean number of milestones: 1946

36. Example with Replanning 12 3 4 5 6 7

37. Another Example with Replanning Total duration : 40 sec

38. mbmb R (m b ) Endgame region Expansive Space m R w (M) R w (m) lookout point lookout

39. Expansive Space Visibility  Reachability S VisibilityReachability

40. Optimality of a Trajectory Often one seeks a trajectory that optimizes a given criterion, e.g.: –smallest number of backup maneuvers, –minimal execution time, –minimal energy consumption Bobrow’s paper + variational techniques

41. Variational Path/Trajectory Optimization  Steepest descent technique.  Parameterize the geometry of a trajectory, e.g., by defining control points through which cubic spines are fitted.  Vary the parameters. For the new values re- compute the optimal control. If better value of criterion, vary further.  No performance guarantee regarding optimality of computed trajectory