1. NUS CS5247 Motion Planning for Humanoid Robots Slides from Li Yunzhen and J. Kuffner

2. NUS CS5247 Papers: Footstep placement J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue. Footstep Planning Among Obstacles for Biped Robots. Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2001. Full-body motion J. Kuffner, S. Kagami, M. Inaba, and H. Inoue. Dynamically-Stable Motion Planning for Humanoid Robots. Proc. Humanoids, 2000

3. NUS CS5247

5. What is Humanoid Robots & its balance constraints? area of support gravity vector center of mass projection of mass center A configuration q is statically-stable if the projection of mass center along the gravity vector lies within the area of support. Human-like Robots

6. NUS CS5247 1. Footstep Placement Goal: In an obstacle-cluttered environment, compute a path from start position to goal region

7. NUS CS5247

8. 1.Footstep Placement—Simplifying Assumption 1.Flat environment floor 2.Stationary obstacles of known position and height 3.Foot placement only on floor surface (not on obstacles) 4.Pre-computed set of footstep placement positions 5.Orientation changes allow direction changing; forward and backward motion also

9. NUS CS5247 Footstep Placement—Simplifying Assumption Pre-calculate statically stable motion trajectories for transition between footstep and use intermediate postures to reduce the number of transition trajectories. Define intermediate postures Qright, Qleft, that are single foot stable. All transitions are Q1, Qleft, Q2, Qright, Q3, etc. Thus, problem is reduced to how to find feet placements (collision-free ) Path = A sequence of feet placements + trajectories for transition between footstep. Qright == stable

10. NUS CS5247 1.Footstep Placement—Overall algorithm

11. NUS CS5247 1.Footstep Placement—Best First Search 1.Generate successor nodes from lowest cost node 2.Prune nodes that result in collisions 3.Continue until a generated successor node falls in goal region 4.Number of possible footstep placements is branching factor of the tree Initial Configuration Red Leaf nodes are pruned from the search

12. NUS CS5247 1.Footstep Placement—Cost Heuristic I.Cost of path to current configuration  Depth of node in the tree  Penalties for orientation changes, backward steps, and poor foot locations II.Cost estimate to reach goal  Straight-line steps from current to goal  Weights favor fewer steps, forward motion  weighting factors are empirically-determined  D() is path length so far, rho() is penalty for rotating or going backward, X() is estimate of remaining cost to goal

13. NUS CS5247

15. 1.Footstep Placement—Collision Checking 2D polygon-polygon intersection test foot vs. attempted position 3D polyhedral minimum-distance determination Robot vs. Obstacle Lazy collision checking used: as new candidate state is chosen, check for collision 15 discrete foot placements, 20 obstacles Search tree has 6,700 nodes. Brute force search would require 10^21 nodes for 18 step path.

16. NUS CS5247

19. Full Body Motion Challenge dues to :  The high number of degrees of freedom  Complex kinematic and dynamic models  Balance constraints that must be carefully maintained in order to prevent the robot from falling down

20. NUS CS5247 Full Body Motion Given two statically-stable configurations, generate a statically stable, collision-free trajectory between them --Kuffner, et. al, Dynamically-stable Motion Planning for Humanoid Robots, 2000.

21. NUS CS5247 Object Manipulation Task: move a target object its new location. Normally 3 steps:  Reach  Transfer  Release

22. Decoupled Approach: Solving the problem by first computing a kinematic path, and subsequently transforming the path into a dynamic trajectory. State-Space Formulation: Searching the system state-space directly by reasoning about the possible controls that can be applied. NUS CS5247 Dynamically Stable Motion Planning Using RRT’s

23. NUS CS5247 Dynamically Stable Motion Planning Using RRT’s Decoupled Approach: Solve for kinematic path, then Transform path to a dynamically stable path

24. Assumptions  Environment Model: Assume that the robot has access to a 3D model of the surrounding environment to be used for collision checking. This model could have been obtained through sensors such as laser rangefinding or stereo vision, or given directly in advance.  Initial Posture: The robot is currently balanced in a collision-free, statically-stable configuration supported by either one or both feet.  Goal Posture: A full-body goal configuration that is both collision- free and statically-stable is specified.  The goal posture may be given explicitly by a human operator, or computed via inverse kinematics or other means. (e.g., for extending a limb towards a target location or object).  Support Base: The location of the supporting foot (or feet in the case of dual-leg support) does not change during the planned motion. NUS CS5247

25. Problem Statement  Robot: a collection of rigid links, with transforms T_i between links. Configuration is q_i, set of joint values, dimension = N (number of DOFs)  Obstacles: Modeled as polygonal meshes. C_free is collision free configurations  C_stable = configs where the robot has 1) COM over support polygon of feet, 2) motor joint torques not exceeded in this configuration.  C_valid = C_free ∩ C_stable is set of configs in C_free that are also stable  Trajectory T: interval over [t_0, t_1] where each configuration q(t) C_valid (collision free and stable)  Dynamic Stability: Given T, assure dynamic stability during transitions between q(t). Post processing step.

26. Distance Metric  RRT will need a distance metric to see how “close” two configs are  With many joints, can take the sum of the absolute differences between joint sets for two configs  Problem: some joints not as important(e.g arm positions while navigating)  Solution: weight each joint as to its importance.  Favor links with greater mass and proximity to torso – ad hoc weightings. NUS CS5247

27. Planning Algorithm  It is unlikely that a configuration picked at random will be collision-free and statically-stable  Use RRT, but instead of finding random path in C_free, and then checking for stability we limit search to random set of C_stable configs  Offline we pre-compute a set of stable configurations, and do the random path planning between these configs  If set of configs fails, can compute more configs.  Single-Leg: Generate random config, assume right foot is on ground, check for static stability. If stable add to C_stable, do same with symmetry for left foot.  For Dual-leg support, assume 1 leg on ground, then find Inverse Kinematics solution for other leg in similar ground contact position NUS CS5247

28. Rapidly-exploring Random Tree (LaValle ’98)  Efficient randomized planner for high- dim. spaces with Algebraic constraints (obstacles) and Differential constraints (due to nonholonomy & dynamics)  Biases exploration towards unexplored portions of the space Rendering of an RRT by James Kuffner

29. NUS CS5247 Full Body Motion—Modified RRT  Randomly select a collision-free, statically stable configuration q. q init q goal q

30. NUS CS5247 Full Body Motion—Modified RRT  Select nearest vertex to q already in RRT (q near ). q init q q near q goal

31. NUS CS5247 Full Body Motion—Modified RRT  Make a fixed-distance motion towards q from q near (q target ). q init q q near q target q goal

32. NUS CS5247 q near Full Body Motion—Modified RRT  Generate q new by filtering path from q near to q target through dynamic balance compensator and add it to the tree. q init q q target q new q goal

33. NUS CS5247 Extend( T,q) q near  NEAREST_NEIGHBOR(q, T) ; If NEW_CONFIG(q,q near,q new ) then T. add_vertex(q new ); T. add_edge(q near,q new ); if qnew = q then return Reached; else return Advanced; return Trapped )

34. NUS CS5247 RRT_CONNECT_STABLE( q init, q goal ) 1. T a.init( q init ); T b.init( q goal ); 2.For k = 1 to K do 3. q rand  RANDOM_STABLE_CONFIG (); 4.if not( EXTEND ( T a, q rand = Trapped ) 5.if( EXTEND ( T b, q new ) = Reached ) 6.return PATH( T a, T b ); 7. SWAP ( T a, T b ); 8.return Failure;

35. NUS CS5247 Full Body Motion—Distance Metric  RRT will need a distance metric to see how “close” two configs are  Don’t want erratic movements from one step to the next.  With many joints, can take the sum of the absolute differences between joint sets for two configs  Encode in the distance metric:  Assign higher relative weights to links with greater mass and proximity to trunk.  A general relative measure of how much the variation of an individual joint parameter affects the overall body posture.

36. NUS CS5247 Full Body Motion—Random Statically stable postures  It is unlikely that a configuration picked at random will be collision-free and statically- stable.  Solution: Generate a set of stable configurations, and randomly choose one from this set then do collision checking.

37. Post Processing the Trajectory  Smoothing: Once we have a trajectory, we can refine it  Attempt to connect different configurations along the path with “straight line” segments in C_valid  Typically eliminates many unnatural poses (e.g. large arm movements)  Dynamics filtering: Body is adjusted iteratively in configurations to achieve dynamic balance  If failure, we slow the robot down to find dynamic stability NUS CS5247

38. RRT VS Expansive Sampling Expansive Space Tree: Merit: Widening Narrow passage RRT Merit: Random Sampling, can find a solution very fast if no narrow pasage.

39. NUS CS5247 Full Body Motion—Random stable postures

40. NUS CS5247 Full Body Motion—Experiment Dynamically-stable planned trajectory for a crouching motion Performance statistics (N = 100 trials)

41. NUS CS5247 Full Body Motion Algorithm for computing dynamically-Stable collision-free trajectories given full-body posture goals. Limitations: Assumes small set of stable configurations Paths may need to be smoothed