CS 326 A: Motion Planning

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 pushingWhy is it useful?- Fewer actuators (less weight)- Design simplicity • - Convenience (think about driving a car with 3 controls!)

4. dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Example: Car-Like Robot f L q f y x Configuration space is 3-dimensional: q = (x, y,q) But control space is 2-dimensional: (v, f) with |v| = sqrt[(dx/dt)2+(dy/dt)2]

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

6. dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Example: Car-Like Robot f L q f y x Lower-bounded turning radius

7. dx sinq – dy cosq = 0 f q dx/dt = v cos q dy/dt = v sin q (x,y,q) L (dx,dy,dq) q dq/dt = (v/L) tan f f y |f| <F x y (dx,dy) x q How Can This Work?Tangent Space/Velocity Space

8. dx sinq – dy cosq = 0 f q dx/dt = v cos q dy/dt = v sin q (x,y,q) L (dx,dy,dq) q dq/dt = (v/L) tan f f y |f| <F x y (dx,dy) x q How Can This Work?Tangent Space/Velocity Space

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. Holonomic path Nonholonomic path Path Transform

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

12. Type 1 Maneuver CYL(x,y,dq,h) h • = 2r/cosdq d = 2r(1/cosdq - 1) dq dq  Allows sidewise motion r

13. Type 2 Maneuver  Allows pure rotation

14. Combination

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

16. Combination

17. Path Examples

18. Drawbacks ofTwo-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 -Y Y X (dt)

20. dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f X: Going straight Y: Turning, angle f |f| <F Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions For example:

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

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

23. endgame region mg mb Control-Based Sampling

24. dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Example • 1. Select a milestone m • 2. Pick v, f, and dt • 3. Integrate motion from m • new configuration

25. Computed Paths Tractor-trailer Car That Can Only Turn Left jmax=45o, jmin=22.5o jmax=45o

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

27. dx sinq – dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/L) tan f |f| <F Nonholonomic vs. Dynamic Constraints • Nonholonomic constraint:q’ = f(q,u)where u is the control input (function of time), with dim(u) < dim(q) u = (v, f)

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) robot obstacles air thrusters gas tank air bearing

32. f a y x Modeling of Robot q = (x,y) s = (q,q’) u = (f,a) x” = (f/m) cosa y” = (f/m) sina f fmax s’ = F(s,u)

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

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. 1 2 3 5 4 6 7 Example with Replanning

37. Another Example with Replanning Total duration : 40 sec

38. R(mb) Rw(M) Rw(m) mb lookout Endgame region m lookout point Expansive Space

39. Expansive Space Visibility  Reachability Visibility Reachability S

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