1. Motion Planning in Stratified Workspace Manifold of a Quadruped Walking Robot Subhrajit Bhattacharya Prof. Vijay Kumar Dr. Sachin Chitta

2. Overview ● Stratified Workspace Manifold – the Workspace Manifold of a Quadruped Robot ● Motion Planning Strategy ● Mathematical description of the Stratified Workspace Manifold – Optimization problem ● Algorithms for Local planning ● Concept of Global planning ● Simulations and Experiments

3. Limitations Of Present Work ● Assumption of a Quasi-static model. ● Assumption of zero weight of the legs. ● Errors/Noise not considered in the main optimization problems. ● Limited experimental success till now because of incomplete feedback correction system. Work on its development is under progress.

4. Introduction The LittleDog Quadruped Robot Stratified Workspace Manifolds Different stages during walking correspond to the different strata (sub-manifolds): Sub-manifolds in a 3 dimensional manifold (needn't be lower dimensional by definition) i. Boundary of a 3 dimensional manifold (a ball) is a 2 dimensional sub-manifold (the surface of sphere). ii. The boundary induces discontinuity in the 3-d manifold.

5. Properties of Stratified Manifold ● Complex Topology characterized by discontinuities giving rise to strata ● System allowed to stay on these sub-manifolds called strata ● Flow allowed along some specific directions on a stratum called foliages/leaves – subset of the tangent space of the stratum

6. Topology of workspace manifold of a quadruped robot ● 18 dimensional workspace: S X P 1 X P 2 X P 3 X P 4 where, ● Discontinuities giving rise to strata – the legs and foot-tips cannot intersect the terrain (essentially a discontinuity in P i ). ● Further discontinuities – the robot body cannot intersect the terrain (discontinuity in S). – the foot-tips should lie within their respective reachable workspaces. – the normal forces at the foot-tips should be positive. – friction force constraints. ● Strata: 4 possible strata corresponding to three legs touching the terrain surface. Each stratum is 18 - 3 = 15 dimensional. ● Foliation: We assume no slippage condition. This leads to foliation of a stratum. Furthermore a finite frictional coefficient induces more discontinuities in the manifold.

7. A low dimensional representation of workspace Manifold of a Quadruped Robot Strata ( S i ) corresponding to 3 legs touching the terrain, i th leg above the terrain. (15 dimensional manifold) Leaves of strata S i Flow can take place only along these allowed directions (9 dimensional space) Leaves of Stratum S 0 corresponding to 4 legs touching the terrain (6 dimensional space) (The movie shows motion along a 3D subset of a leaf)

8. Motion Planning as an Optimization Problem Why an Optimization Problem? Because we have...  Constraint equations and inequalities defining the strata and discontinuities in workspace manifold.  Objectives – moving forward, keeping the body well-aligned with terrain, etc.  Find solutions through workspace manifold towards maximizing objectives and respecting constraints. Strata Leaf Discontinuities Initial State A snippet of a low-dimensional representation of the workspace manifold of a quadruped robot Goal

9. Phases in Optimization  Local planning – Trajectory given, initial state given – find an optimum sequence of steps.  Global planning – Find the trajectory associated with which the cost is minimum (not yet implemented)‏ Assumptions Quasi-static system – no velocity or acceleration terms

10. Notations ● We call the global reference frame G, and the body fixed frame B. ● The position & orientation of body-fixed coordinate – the homogeneous transformation matrix: ● Tip of i th leg:, i = 1, 2, 3, 4 ● A particular state of the robot: ● If r is a point in the space in global coordinates, represents the surface of the terrain. For any point above terrain surface,. ● P is the set of points forming the robot's body polyhedron ● F i is the force acting at the i th foot tip. ● is the 'pseudo-normal' to the terrain surface at the i th foot. ● For a given trajectory τ, we parametrize the trajectory by parameter λ. Thus we write for a point on the trajectory.

11. Forward and Inverse Kinematics ● Forward Kinematics: where, Inverse Kinematics: ● Hence, ● Again, ● Hence, ● Thus can be solved using Paden Kahan Subproblem 3. ● From, using Paden Kahan Subproblem 2, and are solved. pg - 10

12. Computing forces in an indeterminate system using concept of Helicoidal vector field ● where, ● Force and moment balance equations: ● Six equations and six unknowns ● Notes: i. This gives only a particular solution for the forces, while the actual forces still remain indeterminate. ii. This gives solution to the forces irrespective of the number of legs we consider! iii. This gives the minimum norm solution for the wrench vector. iv. This solution is not reliable for very rough terrain surfaces. pg - 12 F i and F j are in a Helicoidal vector field [2] FiFi FjFj riri riri Helicoidal vector field

13. Local Planning Planning sequence of robot states for moving along a given trajectory in the physical space

14. Finding suitable points in the Workspace Manifold for Local Planning ● Initially placing the robot at a point on a stratum with its center coinciding with a point on the given trajectory ● Moving the robot body keeping four feet touching the terrain (movement along leaves of S 0 ) ● Placing foot (Moving along a leaf of S i to reach a new leaf of S 0 from a previous one)

15. Strata Leaf Discontinuities Goal Initial State Phases in Local Planning ● Initial Placement ● Moving Body ● Placing foot

16. Initial Placement ● Required only for simulation – replaced by reading of robot's state in actual run ● We know the point on the given trajectory with which the center of mass should coincide (reduces dimensionality of search-space by 3) ● Search in a 15 dimensional space – computationally highly expensive process SO(3) x R 3 x R 3 x R 3 x R 3 ● Equality constraints: Feet should touch the terrain surface ● Inequality constraints: Robot body shouldn't penetrate terrain, Feet should be within reachable workspace, Normal reaction at feet should be positive, Friction force constraint ● Optimization objectives: Keeping robot body parallel to terrain surface and aligned along the trajectory Keeping the joint angles close to a normal value (pg - 20) [ '*' denotes a given quantity]

17. Moving body – finding a new point along a leaf of S 0 ● We know the leaf along which we need to make the system flow – the footholds of the four legs are given ● Two possible strategies – hard persuasion & soft persuasion ● 3 dimensional search space for hard persuasion – SO(3) 6 dimensional search space for soft persuasion – SO(3) x R 3 While the former is more prone to failures, the later is computationally more expensive. ● Equality constraints: No significant one ● Inequality constraints: Robot body shouldn't penetrate terrain, Feet should be within reachable workspace, Normal reaction at feet should be positive (only in case of soft persuasion), Friction force constraint (only in case of soft persuasion) ● Optimization objectives: Keeping robot body parallel to terrain surface and aligned along the trajectory Keeping the joint angles close to a normal value Moving the center close to (only for soft persuasion) Keeping the forces well inside the friction cone (only for soft persuasion) ( pg - 15-18) (Hard persuasion) (Soft persuasion) [ ' * ' denotes a given quantity]

18. Finding foothold – finding a new point in S if ∩ S 0 along a leaf of S if ● We know the leaf along which we need to make the system flow – the footholds of three legs are given. i f is the foot being placed. ● The leaf along which we can move is 9 dimensional However to reduce the dimensionality of the search space we fix the position & orientation. Thus we have a 3 dimensional search space, R 3 ● Equality constraints: The foot being placed must touch the terrain surface ● Inequality constraints: The foot i f should be within reachable workspace, ● Optimization objectives: The new foothold should be chosen ahead, with an aim to move forward To place the foot on a flat surface Keeping the joint angles of leg i f close to a normal value (pg - 19) [ '*' denotes a given quantity]

19. ● Interpolation: On S i On S 0 ● Search using the hard persuasion strategy: 1 1

20. Concepts of ● Gait Step Sequence ( S ) ● Gait Step Queue ( Q ) Primary difference: S is static, Q is dynamic. In case of failure with a foot we don't modify S and move on to the next foot. But in case of failure with a foot in Q, we push it back by one step and retry again after one step. Specifies the sequence of strata that we should try to follow.

21. Local Planning Algorithm - I (Hard persuasion strategy relying on search) An old algorithm that we were using previously (Skip) (Go to detailed Slide)Go to detailed Slide

22. Local Planning Algorithm - II (Soft persuasion strategy relying on interpolation) pg - 25

23. Simulation - I

24. Run on LittleDog - I (no feedback correction)

25. Simulation - II (larger steps)

26. Run on LittleDog - II (no feedback correction) 2x speed

27. Implementation of a 'quick' Feedback correction system ● Reasons: - Noise in the system - Deviation from out quasi-static assumption - Inconsistency between the issued commands and implemented commands - Weights of the legs - Inaccurate knowledge about robot's dimensions ● Some sort of feedback correction system required for performing experiments with rough terrains ● We are in the process of implementing a correction system using the feedback from MOCAP ● The optimization processes are computationally expensive for obtaining real-time presentable experimental results. Thus the robot's states are planned beforehand in simulation, and this feedback correction system is used to account for any deviation from the desired states.

28. Correction based on feedback from MOCAP ● Strategy: i. The center of the body is moved to the desired center in the global coordinates. The joint angles required for that are calculated accordingly. ii. A new foothold is placed at the desired global coordinate. The joint angles required are calculated accordingly. ● Interpolations done to ensure smooth movements while performing corrections. ● Will work only for small deviations. ● Presently we don't have any theoretical estimate for the range/limit of deviation for which this method will work. ● Convergence to desired states not ensured. ● The performance of presently implemented correction system is not quite satisfactory. Development is in progress.

29. Demonstration of the Feedback Correction System Without Correction With Correction

30. Rough Terrain (Terrain B) pg - 27

31. Run on LittleDog with Terrain B (Correction done using Feedback from the MOCAP) 4x speed

32. A recursive algorithm scheme (not yet implemented) pg - 28

33. Approach towards trajectory optimization Global Optimization (not yet implemented) ● We start with a simple trajectory joining the start and goal positions of the CG. Let this trajectory be, ● We define a generic trajectory ● We can associate a cost required to move along a trajectory τ. Let this cost be ● Taking finite number of terms (say N) in the infinite sum, we now attempt to minimize this cost. Thus solve the optimization problem pg - 30

34. Potential of this method ● Requires that we only mention the desired objectives during motion. ● Planning done automatically respecting all the physical constraints and achieving optimums based on the desired objectives. ● Very few parameters – only the ones involved in the optimization objectives – hence more robust! Drawbacks ● Optimization using MATLAB's Optimization Toolbox is extremely computationally expensive. ● Assumptions made in our model – quasi-static, zero weight of legs.

35. Plans for Future Work ● Improve the feedback correction system ● Implement the recursive algorithm ● Perform the global planning ● Include the dynamic terms in the force equations (this will increase the dimensionality our search space and will induce more constraints in our search processes) ● Reach some conclusion regarding the convergence, stability and performance of the algorithms with mathematical rigor.

36. Acknowledgments ● I would like to express my heartiest thanks to my advisor, Prof. Vijay Kumar. ● I would also like to thank Dr. Sachin Chitta for his kind help.

37. References

38. Thank You

39. Local Planning Algorithm - I (Hard persuasion strategy relying on search) pg - 24

41. Go back (No feedback correction)