Online trajectory generation for wide ditch crossing of biped robots using control constraints

Accepted Manuscript Online trajectory generation for wide ditch crossing of biped robots using control constraints V. Janardhan, R. Prasanth Kumar

PII: DOI: Reference:

S0921-8890(16)30737-0 http://dx.doi.org/10.1016/j.robot.2017.07.014 ROBOT 2885

To appear in:

Robotics and Autonomous Systems

Received date : 23 November 2016 Revised date : 10 June 2017 Accepted date : 28 July 2017 Please cite this article as: V. Janardhan, R.P. Kumar, Online trajectory generation for wide ditch crossing of biped robots using control constraints, Robotics and Autonomous Systems (2017), http://dx.doi.org/10.1016/j.robot.2017.07.014 This is a PDF file of an unedited manuscript that has been accepted for publication. As a service to our customers we are providing this early version of the manuscript. The manuscript will undergo copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please note that during the production process errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.

Online trajectory generation for wide ditch crossing of biped robots using control constraints V. Janardhan1 and R. Prasanth Kumar 1

∗ 1

Department of Mechanical and Aerospace Engineering, Indian Institute of Technology Hyderabad, Kandi, Telangana, 502285, India

∗ R. Prasanth Kumar (Corresponding author), Department of Mechanical and Aerospace Engineering, Indian Institute of Technology Hyderabad, Kandi, Telangana, 502285, India e-mail: [email protected] V. Janardhan e-mail: [email protected]

1

Abstract This work proposes a multibody dynamics approach to generate joint trajectories for a five degrees of freedom biped robot crossing a wide ditch whose width is greater than or equal to the robot’s leg length. Trajectories are generated using control constraints that depend on the horizontal distance traveled by the center of mass and are not explicitly dependent on time. Behavior of the biped robot for various initial postures is studied considering dynamic balance, friction, and impact in order to find the preferred initial postures considering the net energy consumption and peak power requirements at various joints. Several cases of friction, zero moment point location, and the center of mass height variation are considered in the study. Using the proposed approach, feasible trajectories for an adult sized biped robot could be generated for a wide ditch of 1.05 m width at coefficients of friction as low as 0.2. The results obtained are useful for designing reference trajectories and actuation systems for biped robots that need to cross wide ditches or take large steps. Time needed for trajectory generation is found to be sufficiently low for online implementation. Keywords: Biped robot, ditch crossing, wide ditch, online solution, control constraints

1

Introduction

when it starts with different initial postures and for different friction conditions. We also propose a method to find quick solutions for crossing wide ditch using control constraints inspired from multibody dynamics literature. The concept of control constraints for multibody systems has been discussed by many researchers in the literature [5, 6, 7, 8, 9]. The control outputs or performance goals of a mechanical system expressed in terms of system states are treated as constraints of the system called control or servo or program constraints [10]. These constraints are different from contact constraints (hard surfaces, rigid links and slipless rolling contacts etc) which are generally considered in constrained mechanical systems simulation. In this introductory work on using control constraints for trajectory generation, we considered ground level on either side of the wide ditch to be same. This situation may also arise when the robot is walking on level floor and is required to cross a wide area on the floor which is not to be stepped on (because of wet slippery floor, for instance). It is well known fact that when a human being walks or crosses a ditch, motion is not planned with respect to time but based on events. We mimic this behavior in defining control constraints independent of time. We assume Coulomb friction condition between robot foot and ground. During the motion of the biped robot the vertical component of Ground Reaction Force (GRF) should always be positive. Both these are inequality constraints. Solving equations of motion along with these inequalities will become a complementarity problem. Solution of these can be obtained in iterative methods and are time consuming [11, 12, 13]. In order to avoid this, both these inequalities have been replaced with equalities and implemented as control constraints. Finding solutions for equations of motions of biped robot multibody system with the designed control constraints can give both torques and joint trajectories simultaneously. Various initial configurations of the robot and different friction coefficients between foot and ground surface can also be evaluated with respect to net energy consumption and peak power requirements. We also consider impact of the foot with the ground in designing control constraints. Results obtained can help in the proper choice of reference trajectories for control and actuation system for biped robots required to cross wide ditches encountered in many environments. The rest of the paper is organized as follows. Section 2

The ultimate aim of research in humanoid robotics is to replace humans with humanoids in normal as well as unsafe environments for routine or dangerous tasks. In such environments the robot may need to perform tasks some of which humans are capable of doing and some not. The robot may need to move across surfaces which are at the same level such as flat floor, at different levels such as steps, and surfaces which are not continuous such as ditches. Ditches are special obstacles where the robot needs to pay more attention, as the failure can cause severe damage to the robot since ditches can have any depth which is usually random in nature unlike steps. Legged robots have been proposed as a means of carrying loads or traversing across uneven terrains. Choi and Song [1] have reported crossing obstacles like grade, step, ditch and an isolated wall by a hexapod robot with the help of a terrain scanning system that can see and read the terrain geometry. Cheng and Pan [2] have generated gaits for quadruped robot to cross a ditch of width 0.2 m. In biped robots, Fattah and Fakhari [3] have generated trajectories to walk with different step lengths using optimization tools. The authors presented the results for ditch crossing and step climbing for a small ditch and step of size 0.02 m each as case studies. When it comes to two legged robots, very few researchers have studied ditch crossing and none on wide ditch crossing for which the ankle to ankle stretch required is greater than or equal to the leg length. Vundavilli and Pratihar [4] have generated dynamically balanced optimal gaits for ditch crossing by a biped robot using analytical, neural networks and fuzzy logic approaches. The influence of mass positions of various links and hip trajectory on power consumption and stability has been reported. But the the authors have done analysis for only one phase of ditch crossing where the robot places its front leg across the ditch in single support phase. The recovery of the rear leg to the front leg across the ditch and the consequent dynamics and stability have not been analyzed. In the present work we make a thorough study of the behavior of an adult sized planar biped robot crossing a wide ditch. Our goal here is to completely cross a wide ditch including crossing and recovery of the rear leg, of the biped robot with passive and active ankle joints. We present how the robot is going to behave while crossing a wide ditch 2

describes the multibody dynamics model of the biped robot for various phases of ditch crossing. Section 3 describes various cases of crossing a wide ditch and proposed approaches for choosing control constraints. In Section 4, results obtained for various cases of crossing a wide ditch are discussed. Section 5 concludes the paper highlighting the major findings of the present work.

2

Dynamics of the biped robot

2.1

Model of the biped robot

The biped robot considered is a planar biped with seven links. It has two feet, two shanks, two thighs and a torso as shown in Fig. 1. The robot has six joints, two between each foot and shank, two between each shank and thigh, and two between each thigh and the torso. φi is the angle made by the longitudinal axis of link i with horizontal and θi is the relative angle of link i with respect to link i − 1. Following are the assumptions made for crossing a wide ditch: • Any ditch for which maximum ankle to ankle stretch Figure 1: Schematic diagram of biped crossing a wide ditch or distance required to cross the ditch is equal to or beyond the leg length of the biped robot is considered as wide ditch for that robot. • The double support phase while crossing a ditch is instantaneous [14, 15] and stance leg is not going to rebound during impact. • The motion of the biped robot is restricted to sagittal plane only. • Both the feet of the biped robot are mass-less but torque can be applied at the ankles during support phases as in [16]. • Feet of the biped robot are flat always [17]. Figure 2: Phases of the biped robot crossing a wide ditch

• Each link mass is uniformly distributed and its Center of Mass (CoM) is located at the geometric center where the local coordinates axes are defined as shown in Fig. 1.

The equations of motion for the biped robot in single support phase (SSP) can be written as [19], [20]:

• The biped robot is assumed to have point feet when crossing a ditch with passive ankle joints.

˙ θ˙ + G(θ, θ) ˙ = T, M (θ)θ¨ + C(θ, θ)

(1)

where M is the inertia matrix, C is the Coriolis and centrifugal matrix, G is gravity matrix, T is the torque vector for the biped robot. The equations of motion of the biped robot with constraints can be written as

Ditch crossing by a biped robot is divided into two single support phases as shown in Fig. 2 with no double support phase in between. The dashed line in the figure shows the motion of swing foot trajectory. The time for double support phase for crossing wide ditches is small and can be neglected [18]. Phase I: The biped robot keeps one of its legs (stance leg) behind the ditch, moves the other leg (swing leg) over the ditch and places it across the ditch. Phase II: Keeping the swing leg of phase I fixed (which now becomes the stance leg), the robot retrieves the rear leg to the front leg.

˙ θ˙ + G(θ, θ) ˙ = T + J t λ, M (θ)θ¨ + C(θ, θ)

(2)

where J t is the transpose of Jacobian matrix J of the constraints, and λ is the vector of Lagrange multipliers.

2.2

Control constraints and formulation

In the inverse dynamic analysis of a multi-body system, given the motion trajectory of all the bodies consistent with 3

on a gait timing variable p, but not explicitly dependent on time. D is called the Jacobian matrix of the constraint (3) in body coordinates [20], fr , fr are the first and second partial derivatives of the function fr with respect to the variable p. In body coordinate formulation, the variables are the positions and orientation of the local coordinate axes defined at the geometric center of each link shown in Fig. 1. For a planar biped robot each local coordinate axes is located with three parameters (x,y,φ), where x and y are the horizontal and vertical positions of the local coordinate axes, and φ is its orientation with respect to horizontal. The array of body coordinates c is defined as

the contact constraints, forces and/or torques required to obtain that motion trajectory are calculated [20]. These forces and/or torques can be used as nominal control inputs when the multi-body system is controlled to track the associated motion trajectory. In the forward dynamic analysis, given the forces and/or torques, motion trajectory is determined. However, in our current problem, we know neither the motion trajectory nor the required forces and/or torques to accomplish the task of crossing the wide ditch. It is possible to include additional constraints called control constraints [10] in the forward dynamics analysis of multibody systems so that the additional forces that satisfy the control constraints are simultaneously determined during the numerical solution procedure. A method for finding motion trajectories along with torques required, by combining equations of motion and control constraints is proposed by Rosen and Edelstein [6] and will be discussed again here in this section in the context of our biped robot. With this approach actuator torques/forces required can be obtained from reaction torques/forces corresponding to control constraints. Control constraints are the expressions for control outputs or performance goals of a mechanical system in terms of system states. These are also called servo or program constraints. These constraints are different from contact or material constraints. A distinction between these two types constraints is discussed in [10, 9]. The dynamics of the biped robot are formulated in joint coordinates [20], because we want to make the robot follow the required control constraints only by adjusting the joint actuator torques and not by forces. The obtained joint trajectories and the constraint torques can be used as nominal trajectories and nominal torques when a real biped robot is required to cross the wide ditch. Using this approach, generation of joint trajectories and joint torques is not computationally intensive and can be done online when the robot encounters a ditch. The same is not possible if the formulation is done in body coordinates, where we get reaction forces at joints in addition to torques and implementing these reaction forces directly at the joints is not straight forward. Further, body coordinate formulation is computationally expensive for simulation compared to joint coordinates since the number of variables are significantly high. Expressing control constraints directly in joint coordinates is difficult and hence are initially expressed in body coordinates. These constraints can be holonomic or nonholonomic. Constraints in body coordinates are converted to joint coordinates by appropriate transformation [20]. Let a holonomic constraint in body coordinates be of the form

c = [x1 y1 φ1 . . . x5 y5 φ5 ]t . The array of variables in joint coordinates formulation is denoted by θ. The variables in joint coordinate formulation are (see Fig. 1) θ = [θ1 θ2 . . . θ5 ]t The relation between body coordinates and joint coordinates is given as c˙ =B θ˙ c¨ =B θ¨ + B˙ θ˙

(6)

Equation (5) relates velocities in body and joint coordinates where B is the transformation matrix. The procedure for getting the matrix B is given in Appendix A. Using the relations (5) and (6), the constraint equation (4) is converted into joint coordinates as given below ˙ + DB ˙ θ˙ = f  (p)p˙ 2 + f  (p)¨ D(B θ¨ + B˙ θ) p r r Jl θ¨ = Fr

(7)

where Jl = DB,

˙ θ˙ + fr (p)p˙2 + fr (p)¨ p Fr = −(DB˙ + DB)

Jl can be called as the Jacobin matrix of the constraint equation (3) in joint coordinates. In joint coordinates we have θ1 , θ2 , θ3 , θ4 , and θ5 as position state variables equal in number to the degrees of freedom. Thus we have considered a maximum of five control constraints. These constraints are chosen such that they ensure complete crossing of a wide ditch, slip prevention and dynamic balance of the biped robot. The constraints are formulated in such a way that they do not have time as an explicit variable, but a different variable based on the location of the CoM as described in further sections. Holonomic and nonholonomic constraints are converted to joint coordinates and brought to the forms given as

(3) Φ(c) = ξ(c)−fr (p) = 0,  ˙ ˙ ˙ ˙Φ(c, c) ˙ =ξ − fr = Dc˙ − fr (p) = Dc˙ − fr (p)p˙ = 0, ¨ =D¨ Φ c + D˙ c˙ − fr (p)p˙ 2 − fr (p)¨ p = 0, or  2  p D¨ c + D˙ c˙ = fr (p)p˙ + fr (p)¨

(5)

˙ θ, θ˙c , θc ), Jh θ¨ = Fh (θ, ˙ θ, θ˙c , θc ), Jn θ¨ = Fn (θ,

(4)

(8) (9)

where θc the angle made by the line joining ankle and the where ξ is the variable to be controlled described in terms center of mass, it is the gait timing variable. Jh and Jn are of array of body coordinates c, fr is a function dependent Jacobian matrices of the set of holonomic and nonholonomic 4

joining CoM and the ankle of the support foot with respect to vertical which is schematically shown in Fig. 3. The point A, in the figure represents the ankle joint. Foot motions of the biped robot are defined as functions of θc given as

constraints. Fh and Fn are functions representing the right hand side of the constraints in joint coordinates. Combining equations of motion (2) and constraints (8), (9) will give      M J t θ¨ −C θ˙ − G − T = . (10) J 0 λ RHS where



J J= h Jn



and



F RHS = h Fn

xf = f (θc ), and yf = g(θc ).



(11) (12)

These functions are formulated depending on the context in which the robot is going to cross the wide ditch. A key parameter in deciding these foot motion functions is impact ˙ G = Bth M = B t Mb B, C = B t M B, during landing. In general, the foot-ground impact is undeMb = diag[m1 m1 I1 . . . . . . m5 m5 I5 ] sirable in biped robot locomotion due to the sudden change t in joint velocities that could lead to instability. It is for h = [0 − m1 g 0 . . . . . . 0 − m5 g 0] this reason that joint trajectories are carefully planned to Mb is the diagonal matrix formed by the row vector of avoid (or in reality, minimize) the impact forces when the masses and inertias of all links, mi and Ii are the mass and foot touches the ground. We considered the impact event of moment inertia about CoM of the ith link. h represents the the biped robot at the end of phase I assuming the landing vector of external forces acting on the biped robot. The position is known accurately. In this case, it is possible to details of matrices B, J, Jh , Jn , Fh , and Fn are given in plan the foot trajectories such that influence of impact is the Appendix A. Solving (10) for θ¨ and λ, we can simul- nullified. Due to impact, the angular velocities of the joints vary taneously find joint motion and actuator torques required for the biped robot to cross the wide ditch satisfying all the after impact. However, the joint angles remain same. The post impact velocities of the joints depend on the swing control constraints. foot velocity and the configuration of the robot before impact [21, 22, 15]. By making the swing foot velocity of the 3 Biped robot crossing a wide ditch robot before impact as zero, post impact velocities of the joints become equal to the pre-impact velocities. Considin various contexts ering this phenomenon, the authors have designed the foot Important issues to be considered for a biped robot crossing trajectories such that the landing velocity of the swing foot a wide ditch are foot trajectories of the swing foot, dynamic is zero in both horizontal and vertical directions to nullify balance (stability or ZMP), friction and total kinetic energy the influence of impact. of the biped robot. Dynamic balance The dynamic balance of the biped robot is ensured using Zero Moment Point (ZMP) criteria. ZMP is a point on the ground contact region of the biped robot, where the influence of all contact forces acting on the biped robot foot can be replaced by one single force [23]. About this point the net moment caused by the inertial forces and gravity forces along the horizontal axes is zero. For the biped robot to be in dynamic balance ZMP should always be kept within the foot support polygon. Schematic diagram of the biped robot crossing a wide ditch is given in Fig. 4. A is a point on the foot where ankle joint exists. Rax and Ray are the net forces acting at the ankle joint from the other links above the foot of the biped robot. R is the Figure 3: Schematic diagram of gait timing variable ground reaction force acting on the stance foot. Ta is the moment or the torque applied at the ankle joint on the foot Foot trajectories Foot trajectories in general are defined through actuator. Considering moment equilibrium of the with respect to time. For doing this we have to predefine foot about point A, neglecting the mass and thickness of the or presume the time required for completing the task, but foot, the corresponding moment equation is given in (13). there is no standard rule defined for deciding the time reBased on the definition of ZMP, it can be observed from quired for completing a locomotion task of the biped robot. Fig. 4 that in SSP, the x component of ZMP, ZM Px for a Humans never presume precise time for completing locomoplanar biped robot is obtained by dividing the ankle torque tion tasks but only decide to complete the task either fast with vertical component of Ground Reaction Force (GRF) or slow. Depending on this fuzzy decision, one moves his [24], [25]. By making the ankle torque zero, ZM Px stays at limbs relative to each other. Inspired from this we propose the ankle ensuring maximum dynamic stability. This criteto define foot motion not with respect to time but with reria has been implemented in the present work for ensuring spect to a parameter θc . It is the angle made by the line 5

inside a triangle with the semi vertex angle same as that of friction cone. The constraints for keeping GRF within the friction cone assuming Coulomb friction between foot and floor are given by

dynamic stability of the planar biped robot with passive ankle joints. We assume the biped robot to have point feet when ankle torque is zero.

|Rx | − μRy ≤ 0, Ry ≥ 0,

(14) (15)

where Rx and Ry are the horizontal and vertical components of ground reaction force. Equations (14) and (15) describe the Coulomb friction condition and unilateral constraint respectively. If condition (15) is not true, it means the ground pulls the foot which does not happen unless the foot is glued to the ground [28]. Since both the conditions (14) and (15) are inequality constraints, incorporating these directly into the system of equations (10) will lead to complementarity problem which requires constrained optiFigure 4: Free body diagram of biped robot stance foot in mization or iterative methods [11, 12, 13] to solve. In this SSP work we have replaced the conditions (14) and (15) with equality constraints that will always satisfy the inequalities as discussed in the coming sections. In addition to above mentioned considerations, the biped Ta − Ry d = 0 robot needs to be brought to rest by the time it completes Ta phase-II. In doing that biped robot should not slip (translad = ZM Px = Ry tional stability) and should be rotationally stable. To satisfy both the criteria, the concepts of friction cone and cenMaking the ankle torque zero troidal angular momentum have been combined in bringing ZM Px = d = 0 (13) the biped robot to complete rest at the end of phase-II. Biped robot does not slip as long as the Ground ReacFriction Friction is a major consideration during the cross- tion Force(GRF) is within the friction cone. As stated by ing of a wide ditch because of the large distance robot’s Goswami and Kallem [29], the biped robot is rotationally center of mass is required to travel. It is necessary to en- stable if the rate of change of Centroidal Angular Momensure that robot is not going to slip or slide while crossing tum (CAM) is zero. This happens when the GRF is made the ditch. We assumed Coulomb friction between the robot to pass through the CoM. At the same time, it should be enfoot and the ground. The robot does not slip as long as the sured that GRF is within the friction cone for translational net GRF is within the friction cone. The bounds on net stability. As the robot’s CoM moves forward during crossGRF acting at the point of contact to avoid slipping can ing of a wide ditch, there is a possibility that CoM leaves be schematically represented by friction cone as shown in the friction cone. When CoM is outside the friction cone the figure Fig. 5. It is a right circular cone with vertex at and GRF is within or on the friction cone, rate of change of CAM is nonzero and the robot may have tendency to tip. However, it does not fall as long as the ZMP is within the foot support polygon. At the end of phase-II, not only the rate of change of CAM but also CAM has to be brought to zero to ensure complete stability of the biped robot. These issues are addressed in the following sub-sections.

3.1

Ditch crossing keeping the CoM within friction cone

The advantage of keeping the CoM within the friction cone is that we can provide maximum translational as well as rotational stability of the biped robot making the GRF pass Figure 5: Friction cone due to the net GRF acting on foot through the CoM almost all of the time. In Phase I the biped robot starts from rest with xc , the x-coordinate of the point where the net GRF acts and the axis of the cone CoM, at the ankle of the supporting foot. The phase ends is in the direction of the normal component of GRF (Ry ). when x reaches half the width of the ditch. At this instant, c The semi vertex angle (φ) of the cone is equal to tan−1 μ, the swing foot lands in front of the ditch and the support where μ is the coefficient of static friction [26, 27]. For a shifts from the rear foot to the front. Phase II starts with planar case to avoid slipping, the net GRF vector should be 6

The second and third control constraints are defined on horizontal and vertical swing foot motion as

xc at half the ditch width and ends when it reaches a position directly above the ankle of the support foot. All the control constraints are defined with respect to the variable θc . We formulated a total of five control constraints: one for ensuring the vertical component of GRF to be positive, two of them are for foot motion considering avoiding impact during landing, one for ensuring dynamic balance of the biped robot and the other for preventing sliding motion of the biped robot. These constraints are described for the two phases as follows:

xf 1 = f1 (θc ), and yf 1 = g1 (θc ).

(24) (25)

where xf 1 and yf 1 are the foot trajectories during phase I. The functions f1 and g1 are sixth degree polynomials defined as 6 

m¨ yc + mg = Ry , Ry = mg,

(18) (19) yf 1

we can see that the vertical component of GRF will remain constant and equal to the total weight of the biped robot as shown in (19). This way the inequality constraint on Ry is simplified to equality constraint. This also ensures that though the ZMP is at the ankle inside the foot support polygon, the robot does not fall with CoM touching the ground. The constraint (17) is in body coordinates and it is double differentiated and converted into joint coordinates and transformed to the form

y˙ f 1 y¨f 1

ai θci , and g1 (θc ) =

6 

bi θci , (26) Phase I One of the important issues to be considered duri=0 i=0 ing biped locomotion is the vertical component of Ground Reaction Force(GRF). It should always be positive which is satisfying the boundary conditions specified as follows. ⎧ expressed as an inequality constraint. In order to achieve ⎪ ⎪ this we kept yc , CoM height from the ground, constant in ⎨ 0 θc = 0 w both the phases of ditch crossing. The corresponding con) xf 1 = f1 (θc ) = w2 θc = tan−1 ( 4y c ⎪ ⎪ w −1 trol constraint is given by ⎩w θc = tan ( ) 2yc  (16) yc = k 1 , 0 θc = 0 x˙ f 1 = f1 (θc )θ˙c = w or y¨c = 0, (17) ) 0 θc = tan−1 ( 2y c  where k1 is the initial height of CoM. 0 θc = 0 2 x ¨f 1 = f1 (θc )θ¨c + f1 (θc )θ˙c = (27) From the equation of motion for CoM along vertical diw ) 0 θc = tan−1 ( 2y c rection f1 (θc ) =

⎧ ⎪ ⎪ ⎨0 = g1 (θc ) = k2 ⎪ ⎪ ⎩0  0 = g1 (θc )θ˙c = 0

θc = 0 w θc = tan−1 ( 4y ) c −1 w θc = tan ( 2yc )

θc = 0 w ) θc = tan−1 ( 2y c  0 θc = 0 2 = g1 (θc )θ¨c + g1 (θc )θ˙c = w ) 0 θc = tan−1 ( 2y c

(28)

Here  and  are used to indicate the first second partial derivatives. f1 , g1 and f1 , g1 represents the first and second ˙ = 0. (20) partial derives y¨c = Jyc θ¨ + Fyc (θ, θ) of the functions f1 and g1 w.t.to θc . The in (28) is the height of the robot foot above constant k 2 The derivation for (20) and the expressions for the functions the ground level at the particular value of θc specified. The Jyc and Fyc are given in appendix A. to a6 and b0 to b6 are found by solving the coefficients a 0 Let w be the width of the ditch considered for crossing seven conditions in each of the equations (27) and (28). and μ be the friction coefficient on both sides of the ditch. Phase I begins when θc is zero and ends when θc is equal Then the limit on θc to keep the CoM within friction cone −1 w ( ) or x is at half the ditch width. It can be to tan c 2yc is given by observed from boundary conditions in equations (27) and (21) (28) that the velocities and accelerations of the foot in both tan θc ≤ μ. directions are zero at the end of the phase I ensuring zero Using this inequality at the end of phase I, impact landing. The second and third control constraints in acceleration xc w = ≤ μ, (22) form are given by tan θc = yc 2yc 2 x ¨f 1 = f1 (θc )θ¨c + f1 (θc )θ˙c ,

and the condition on height of the CoM can be written as w yc ≥ , 2μ

2

y¨f 1 = g1 (θc )θ¨c + g1 (θc )θ˙c .

(23)

(29) (30)

which gives vertical lower limit for the CoM for the given To transform these equations to joint coordinates, we will replace the acceleration θ¨c in terms of other velocities by ditch width and friction coefficient. 7

double differentiating the expression xc = yc tan θc as follows: θ¨c =

x ¨c y¨c tan θc 2y˙ c θ˙c − − − 2 tan θc θ˙c2 yc sec2 θc yc sec2 θc yc

where M1 , C1 and G1 are the first rows of the matrices M , C and G of (1), and T1 is the torque corresponding to the ankle joint of the supporting foot. By making the ankle torque T1 as zero, the equation is transformed to the form given as

(31)

Using this equation in (29) and (30), we get 

˙ θ˙ − G1 (θ, θ). ˙ M1 (θ)θ¨ = −C1 (θ, θ)

2



x ¨ f 1 − α1 x ¨c + α2 y¨c =(f1 (θc ) − 2f1 (θc ) tan θc )θ˙c −  2f (θc )y˙ c θ˙c 1

yc   2 y¨f 1 − β1 x ¨c + β2 y¨c =(g1 (θc ) − 2g1 (θc ) tan θc )θ˙c −  2g (θc )y˙ c θ˙c

1

yc

This is a nonholonomic control constraint already in joint coordinates. We found that without giving a horizontal force to the biped robot, it tends to stay where it is. If a small horizontal force is applied in the beginning of phase 1, it generates centroidal angular momentum since the GRF vector does not pass through the CoM when it is at initial (xc = 0) location. This is desirable though it causes non-zero CAM. Too small horizontal force on the CoM can cause the hip lag behind the swing foot and moving the leg closer to singularity due to opening up of the swing leg’s knee. On the higher side, the horizontal reaction force Rx , and hence the horizontal CoM acceleration, is limited due to friction condition as:

(32)

(33)

where 

α1 =

f1 (θc ) , yc sec2 θc



β1 =



g1 (θc ) , yc sec2 θc

α2 =

f1 (θc ) tan θc , yc sec2 θc

and β2 =

g1 (θc ) tan θc . yc sec2 θc



|Rx | ≤ μmg or |m¨ xc | ≤ μmg ⇒ |¨ xc | ≤ μg

Using y˙ c = y¨c = 0, following simplified expressions are obtained: θ¨c =

x ¨c − 2 tan θc θ˙c2 yc sec2 θc 



¨c =(g1 (θc ) − 2g1 (θc ) tan θc )θ˙c y¨f 1 − β1 x

2

(35) (36)

By writing the positions of swing foot and CoM in terms joint coordinates, these equations are transformed to the form ˙ θc , θ˙c ), Jx1 θ¨ = Fp1 (θ, θ, ˙ θc , θ˙c ). Jy1 θ¨ = Gp1 (θ, θ,

(37) (38)

Jx1 =(D1 − α1 D3 )B

  2 Fp1 =(f1 (θc ) − 2f1 (θc ) tan θc )θ˙c − ((D1 B˙ + D˙ 1 B)− α1 (D3 B˙ + D˙ 3 B))θ˙





(42)

where k4 is the horizontal distance of CoM up to which an initial drive force of magnitude k3 g is provided, and k3 is the initial drive force coefficient. After xc reaches k4 , acceleration is provided such that the reaction force passes through CoM. We choose k4 as 5 mm in all the cases we have considered for ditch crossing. The initial drive force to be applied varies from case to case and also depends on the initial posture of the robot. The value of k3 is chosen such that its absolute value is less than or equal to μ.

where

Jy1 =(D2 − β1 D3 )B

(41)

The fifth control constraint is therefore introduced to give a forward push to the robot, and to ensure that friction condition is also satisfied as follows:  for 0 ≤ θc ≤ tan−1 ( kyc4 ) k3 g (43) x ¨c = w ) (tan θc )g for tan−1 ( kyc4 ) < θc ≤ tan−1 ( 2y c

(34)

  2 x ¨ f 1 − α1 x ¨c =(f1 (θc ) − 2f1 (θc ) tan θc )θ˙c

(40)

Phase II In phase 2, the support changes from rear leg to front leg and the origin is shifted to the ankle of the support leg. The horizontal coordinate of CoM is half the ditch width behind the origin at the beginning, and reaches zero by the end when the robot comes to rest. The first control constraint for phase II, similar to phase I, is constant CoM height equal to k1 . The second and third control constraints are on the rear ankle motion defined as

2

Gp1 =(g1 (θc ) − 2g1 (θc ) tan θc )θ˙c − ((D2 B˙ + D˙ 2 B)− β1 (D3 B˙ + D˙ 3 B))θ˙

Here Di is the ith row of the constraint Jacobian matrix D given in appendix A. Complete derivation for converting the equations (35) and (36) into equations (37) and (38) are xf 2 = f2 (θc ), and (44) given in appendix A. yf 2 = g2 (θc ), (45) The fourth control constraint is on keeping the ZMP at the ankle joint by maintaining zero ankle torque. The first where f2 and g2 are sixth order polynomials defined as row of the set of equations in (1) corresponds to the ankle 6 6   joint, it is given as ci θci , and g2 (θc ) = di θci , (46) f2 (θc ) = ˙ θ˙ + G1 (θ, θ) ˙ = T1 i=0 i=0 (39) M1 (θ)θ¨ + C1 (θ, θ) 8

satisfying the boundary conditions defined as follows. ⎧ −1 −w ⎪ ⎪ ⎨−dw θc = tan ( 2yc ) xf 2 = f2 (θc ) = −dw θc = tan−1 ( −w 2 4yc ) ⎪ ⎪ ⎩0 θc = 0  0 θc = tan−1 ( −w  2yc ) x˙ f 2 = f2 (θc )θ˙c = 0 θc = 0  0 θc = tan−1 ( −w 2   2yc ) ¨ ˙ x ¨f 2 = f2 (θc )θc + f2 (θc )θc = (47) 0 θc = 0

yf 2

y˙ f 2 y¨f 2

⎧ ⎪ ⎪ ⎨0 = g2 (θc ) = k5 ⎪ ⎪ ⎩0  0  = g2 (θc )θ˙c = 0

friction surfaces the approach followed in the previous subsections has to be modified in the region where the CoM goes outside the friction cone. The constraints on height of CoM yc (16), foot trajectories (24), (25), (44), (45), and the ZMP (40) for phase-I and II remain same. The fifth control constraint on horizontal acceleration of CoM is modified in phase-I and phase-II such that the GRF vector remains on the friction cone when the CoM is outside the friction cone. The constraint for phase-I is given as ⎧ ⎪ for 0 ≤ θc ≤ tan−1 ( kyc4 ) ⎪ ⎨ k3 g x ¨c = (tan θc )g for tan−1 ( kyc4 ) < θc ≤ tan−1 μ (50) ⎪ ⎪ −1 −1 w ⎩μg for tan μ < θc ≤ tan ( ) 2yc

θc = tan−1 ( −w 2yc ) −1 −w θc = tan ( 4yc ) θc = 0

θc = tan−1 ( −w 2yc ) θc = 0  0 θc = tan−1 ( −w 2   2yc ) ¨ ˙ = g2 (θc )θc + g2 (θc )θc = 0 θc = 0

Similarly, the fifth control constraint for phase II is given

by ⎧ ⎪ ⎪ ⎪−μg ⎪ ⎨(tan θ )g c x ¨c = ⎪−k3 g ⎪ ⎪ ⎪ ⎩0

(48)

The height of the foot k5 above the ground level in the specified location of θc need not be same as k2 defined in phase I. The swing foot motion constraints (44) and (45) are transformed to the acceleration form in joint coordinates as given in equations (37) and (38) following the same procedure specified in phase I. The fourth constraint is for making stance ankle torque zero as described in phase I. The fifth constraint is on CoM horizontal acceleration given as ⎧ −1 −w −1 −k4 ⎪ ⎪ ⎨(tan θc )g for tan ( 2yc ) ≤ θc ≤ tan ( yc ) 4 x ¨c = −k3 g for tan−1 ( −k ˙c ≥ 0 yc ) < θc and x ⎪ ⎪ ⎩0 for all other cases

3.2.2

−1 for tan−1 ( −w μ 2yc ) ≤ θc ≤ − tan −1 −1 −k4 for − tan μ < θc ≤ tan ( yc ) (51) 4 for tan−1 ( −k ˙c ≥ 0 yc ) < θc and x for all other cases

With active ankle joints

Although we consider active ankle joints where the stance ankle is supplied with torque, feet are considered to be massless. As a result of nonzero torque at the ankle, ZMP moves away from ankle. Here we fix the location of ZMP at some distance from the ankle and maintain its location during the stance phase. The study also includes varying CoM height by formulating appropriate control constraints.

Phase-I When the height of CoM yc was constant in earlier cases, xc and θc could be related by xc = yc tan θc . The gait timing variable chosen was θc . However, since we consider varying CoM height during crossing, choosing xc as (49) gait timing variable rather than θc gives simpler expressions for constraints. Hence, the first control constraint is formuThis approach is applicable for particular surface as along lated on yc in terms of xc rather than θc . The constraint is w as the lower limit on yc ( 2μ )given in (23) is satisfied. For a fourth order polynomial given by: some of the initial configurations of the biped robot this lower limit may be violated when the friction is low. yc = fy (xc ), or, 

3.2 3.2.1



xc + fy (xc )x˙ 2c , y¨c = fy (xc )¨

Ditch crossing when the CoM goes out satisfying the boundary conditions of friction cone 

With passive ankle joints

yc0 xc = Zr yc = yc0 − ycd xc = W For low friction surfaces the size of friction cone is smaller 2 + Zr  and the CoM may go out of friction cone for the given wide = 0 xc = Zr ditch. In such cases we cannot make the GRF pass through y˙ c = = 0 xc = W the CoM outside the friction cone due to the danger of slip2 + Zr ping. Further, it also makes the rate of change of angular y¨c = 0 xc = Z r (52) momentum about the CoM nonzero. There can be rotational instability due to which the biped robot stretches or shrinks its limbs with respect to the CoM. This is accept- y is the height of the y in the beginning of phase-I. y is c0 c cd able as long as the robot does not fall into the ditch or joint the specified displacement of y by the end of phase-I. c limits are not reached. For crossing a wide ditch on low 9

The second and third control constraints for horizontal and vertical motion of swing foot ankle for phase-I (and also phase II) are same as discussed for earlier cases. The fourth control constraint on the ZMP is formulated such that it is at a specified location within the foot support polygon as follows:   ¨i + mi (xi + Zr )¨ yi + Ii φ¨i = mi g(Zr − xi ) −mi yi x (53) Here mi is the mass of the ith link, Zr is the specified location of ZMP. Further the constraint is converted into joint coordinates. The initial posture of the biped robot is chosen such that the CoM is directly above the ZMP at Zr . The fifth constraint on horizontal acceleration of the CoM is as follows: ⎧ ⎪ ⎪ yc + g) for Zr ≤ xc ≤ Zr + k4 ⎨k3 (¨ x ¨c = (tan θr )(¨ yc + g) for Zr + k4 < xc ≤ Zr + μyc ⎪ ⎪ ⎩μ(¨ yc + g) for Zr + μyc < xc ≤ W 2 + Zr

the ditch with active ankle joints allowing CoM go out of friction cone and varying yc . We choose this case as all other cases are a subset of this. For crossing a wide ditch, considering friction, impact, and dynamic balance, we designed a total five control constraints. The first constraint is for controlling the height of CoM. Second and third are for generating foot motion with respect to a parameter θc . The fourth constraint is for controlling the location of ZMP, and the fifth constraint is for defining the horizontal motion of CoM. Generalized expressions for the five control constraints in acceleration form are restated below as y¨c − fy (xc )¨ xc = fy (xc )x˙ 2c x ¨f − f  (θc )θ¨c = f  (θc )θ˙2 1

1

c

y¨f − g1 (θc )θ¨c = g1 (θc )θ˙c2   ¨i + mi (xi − Zr )¨ yi + Ii φ¨i = mi g(Zr − xi ) −mi yi x x ¨c − kf y¨c = kf g

(54)

Here, θr is the angle made by the line joining the CoM and ZMP rather than ankle since GRF acts at the point where ZMP is located, and the friction cone’s vertex should be considered at the ZMP. The constraints on accelerations of xc and yc can be combined to cross check unilateral nature of GRF in advance for different values of ycd .

(56)

To find the partial derivatives of f1 , g1 , fy we first find the polynomial coefficients of the functions for the given ditch width and initial posture. The parameter kf is chosen such that Coulomb friction and unilateral GRF constraints are satisfied. To choose kf , in each of the single support phases the span of horizontal CoM is divided into three regions as shown in Fig. 6. In the region-1 an initial drive force is

Phase-II In phase-II, ZMP location is specified in front of support foot ankle. Control constraint on ZMP is same as in phase-I. CoM height yc is a polynomial of degree five satisfying the boundary conditions given by  yc0 − ycd xc = − W 2 + Zr yc = yc0 xc = Zr  = 0 xc = − W 2 + Zr y˙ c = = 0 xc = Z r  = y¨c1 xc = − W 2 + Zr y¨c = =0 xc = Zr where y¨c1 is the acceleration of yc at the end of phase-I. The boundary conditions are same as phase I except that we have to consider acceleration of yc at the end of phase II. The constraint on acceleration of xc as follows, ⎧ ⎪ −μ(¨ yc + g) for xc ≤ −μyc + Zr ⎪ ⎪ ⎪ ⎨(tan θ )(¨ y + g) for − μyc + Zr < xc < Zr − k4 r c x ¨c = ⎪ −k3 (¨ yc + g) for Z r − k 4 ≤ xc ≤ Zr ⎪ ⎪ ⎪ ⎩0 for other values of xc (55)

Figure 6: Regions of CoM and direction of GRF in crossing a wide ditch

applied to make the robot start moving which is applied till xc moves a distance of k4 (xc = Zr + k4 ). In Fig. 6, Zr is taken as zero. In region-2, we design the motion of CoM such that GRF passes through CoM till xc is within the friction cone (xc = Zr + μyc ). In region-3, the CoM moves out of the friction cone, during which the motion of CoM 3.3 Summary of the proposed approach is designed such that GRF stays at the edge of the friction w Various steps involved in crossing a wide ditch for different cone till CoM moves half the distance (xc = Zr + 2 ) from for the three regions in the starting point. The values of k f cases are explained taking the case where the robot crosses 10

ized constraints from which we can obtain constraints for each case are given by the set of equations (56). The simple conditions or rules to be applied to the set of equations (56) to get control constraints for each case are given in Table 2. In the table, the condition Zr = 0 is equivalent to making ankle torque zero when the origin is at the ankle. By taking ycd equal to zero the function fy (xc ) becomes constant(yc0 ). The foot trajectory constraints with respect to θc are same for all the cases.

4

The analysis on crossing a wide ditch by a biped robot is done to find the motion of the various links, energy consumption, peak power required at different joints, and time for completing the task for various initial postures and at different friction conditions. This analysis is classified based on the location of the CoM with respect to the friction cone of the supporting foot. It is also focused on finding efficient feasible solutions in short time. The specifications of the biped robot considered for analysis is given in Table 3. The specification of the biped robot provided in the table is according to the relative weight and length of body segments for adult sized men given in [30]. The analysis is carried out for crossing a ditch of width 1.05 m, which is equal to the length of the single leg of the biped robot. We define a ditch as wide, when the ankle to ankle distance required to cross the ditch is equal to or greater than the leg length of the biped robot. The maximum horizontal distance between the ankles at the end of phase I is treated as the width of the ditch crossed by the biped robot. Actual ditch width may be be smaller if ZMP is allowed to move away from ankle.

Figure 7: Flowchart of the algorithm implemented for phase I each phase are provided in Table 1. However, it can also be found from constraint (54) for phase I and constraint (55) for phase II along with the bounds of each region.

Phase I Phase II

Region-1

Region-2

Region-3

k3 −k3

tan(θr ) tan(θr )

μ −μ

Results and Discussions

Table 1: Values of kf for different regions in each phase of ditch crossing

4.1

θr becomes θc when Zr is zero. After finding the various function coefficients and kf , we find the constraint Jacobian matrix (J) in joint coordinates as given in the Appendix A. To find the joint motions and joint torques the differential equation (10) has to be solved. For that, we also need to find the matrices M , C, G as defined in equation (10). The matrix RHS in the equation is also given in Appendix A. For phase I the differential equation has to be solved till xc moves half the ditch width. The flow chart of the algorithm implemented in finding the solutions is given in Fig. 7. Once the stopping criteria for phase I is satisfied then phase II starts. The initial state of the robot for phase II, is the state of the robot at the end of phase I. The steps involved in phase II are same as phase I. The functions f1 and g1 become f2 and g2 in phase II and the values of kf changes in each region as given in Table 1. The same flowchart (Fig. 7) is applicable for phase II with the stopping criteria that, the robot should come to complete rest landing the rear leg in front of the ditch. Until now we have discussed many cases of ditch crossing and constructed the constraints case by case. The general-

Ditch crossing keeping the CoM within the friction cone with passive ankles

The advantage of keeping the CoM always within the friction cone is that we can provide maximum translational as well as the rotational stability to the biped robot. One of the necessary conditions to be met for this case is given in equation 23. Satisfying the necessary condition the initial posture with knee angle 80 degrees and torso angles 60 and 90 degrees are chosen for making biped robot cross a wide ditch of width 1.05 m at coefficient of friction 0.6. The stick diagrams of biped robot crossing a wide ditch with the aforementioned initial postures are given in Fig. 8. The swing foot in Phase I lands in front of the ditch when xc reaches half of the ditch. In phase II the swing foot of phase I becomes stance foot and the stance foot of phase I becomes swing foot. The swing foot moves to the location of the stance foot ankle as xc approaches zero. Figure 9 shows the motion of line joining the CoM and ankle along with the friction cone for the same initial postures shown in Fig. 8. The GRF vector is always within the friction cone and passes through the CoM except for the initial and final 5 mm horizontal movement of CoM ensuring maximum stability.

11

S.No

Ditch crossing Case

Conditions or rules

1

Keeping CoM with in friction cone

Choose initial posture of the robot such that w yc0 ≥ 2μ (when yc = constant) w yc0 ≥ ycd + 2μ (when yc is varying)

2

Allowing CoM go out of friction Cone with passive ankle joints

3

Allowing CoM go out of friction Cone with active ankle joints

Zr = 0 and ycd = 0 (when yc = constant) Zr = 0 and ycd = 0 (when yc is varying) Zr = 0 and ycd = 0 (when yc = constant) Zr = 0 and ycd = 0 (when yc is varying)

Link No

Link

Length (m)

mass (kg)

0 1 2 3 4 5 6

Foot(Left) Shank(Left) Thigh(Left) Torso Thigh(Right) Shank(Right) Foot(Right)

0.3 0.55 0.50 0.80 0.50 0.55 0.3

0 3 6 30 6 3 0

Y-Position(m)

Table 2: Simple rules for getting control constraints for various cases

1.5

1.5

1

1

0.5

0.5

0

0

-0.5

Table 3: Specifications of the biped robot links

-0.5

0.5 1 X-Position(m)

1.5

-0.5

0

0.5 1 X-Position(m)

1.5

0

0.5 1 X-Position(m)

1.5

(a)

Y-Position(m)

The plot of the CAM and Kinetic Energy(KE) of the biped robot for these same cases are given in Fig 10 (a) and (b). The CAM is zero in the beginning of phase I. Since the GRF vector is maintained constant till CoM reaches 5 mm, it does not necessarily pass through the CoM resulting in nonzero rate of change of CAM. The CAM increases when GRF vector is in front of CoM and decreases when it is behind the CoM. After CoM reaches 5 mm, GRF vector is made to pass through CoM till xc is 5 mm behind the ankle of the front foot in phase II. After that the curve is symmetric with phase I, since the horizontal force component of the GRF applied is equal and opposite to that applied in phase I. It can be observed that KE curve starts at zero and increases during Phase I, due to the applied acceleration on CoM. It decreases during phase-II and finally reaches zero at the end of phase II, due to the deceleration of CoM in phase II. In Fig. 9 as the torso angle reduces, the CoM line goes closer towards the edge of the friction cone. This is evident from (23). As the initial height of CoM reduces with torso angle, it reaches the boundary of the necessary condition for yc . If we further reduce the torso angle, CoM goes out of the friction cone.

-0.5 0

1.5

1.5

1

1

0.5

0.5

0

0

-0.5

-0.5

-0.5

0

0.5 1 X-Position(m)

1.5

-0.5

(b)

Figure 8: Stick diagram for Phase-I (left) Phase-II (right) at μ = 0.6, with initial knee angle 80◦ , and for initial torso angles (a) 90◦ (b) 60◦

for the given ditch width as the height of CoM in the initial posture is reduced. Stick diagram of the biped robot CoM with respect to friction cone for torso angles 30◦ and 0◦ are given in Fig. 11 (a) and (b). In figure it can be observed that GRF passes through the CoM when the CoM is within the friction cone, and stays on the edge of the friction cone when the CoM is outside the friction cone. 4.2 Ditch crossing allowing the CoM go out Plots of the CAM for these cases are given in Fig. 12. of the friction cone Variation in the plot of the CAM from the one presented in Fig. 10 (a) can be found, especially in the middle of the plot 4.2.1 With passive ankle joints which belongs to the region where the CoM is outside the Continuing from the previous results if we keep the knee an- friction cone. In this region the GRF does not pass through gle at 80◦ and further reduce the torso angle, the line joining CoM and thus CAM is not conserved. the CoM and the stance foot ankle crosses the friction cone 12

1 0.8 0.5

100 o

0.4 0.2


2-80 3-90

0 o


2-80 3-90

o

o


-80o  -60o 2

3

50


2-80o 3-60o

0

-0.5

0

2 Time(s)

-0.2

0

4

0

2 Time(s)

(a)

-0.4

0

0.5 X-Position(m)

1

4

(b)

Figure 10: Plots of (a) CAM and (b) KE of the biped robot while crossing a wide ditch with knee angle 80◦ and torso angles 90◦ and 60◦ at μ = 0.6 and keeping the CoM within the friction cone

-0.6 -0.5

KE(Nm)

CAM(Nm
)

Y-Position(m)

0.6

1.5

(a) 1 0.8

0.4 0.2

1

0

0.8

-0.2

0.6

-0.4

0.4

Y-Position(m)

Y-Position(m)

0.6

-0.6 -0.5

0

0.5 X-Position(m)

1

1.5

0.2 0 -0.2

(b)

-0.4

Figure 9: CoM plot of the biped robot crossing a wide ditch w.r.to friction cone when knee angle 80o and torso angle (a) 90◦ (b) 60◦ at μ = 0.6

-0.6 -0.5

0

0.5 X-Position(m)

1

1.5

1

1.5

(a) 1 0.8 0.6 Y-Position(m)

A detailed study for this context at coefficients of frictions 0.5, 0.3 and 0.2 has been carried out to get better understanding of biped robot behavior for various initial conditions. The stick diagrams of the biped robot with initial posture with knee angle at 80◦ and with the torso angles varying from 90◦ to 0◦ at coefficient of friction 0.5, 0.3, and 0.2 are shown in Fig. 13 (a) to (l).

0.4 0.2 0 -0.2 -0.4 -0.6 -0.5

0

0.5 X-Position(m)

(b)

Figure 11: CoM plot of the biped robot crossing a wide ditch w.r.to friction cone when knee angle 80o and torso angle (a) 30o (b)0o at μ = 0.6

13

2

150

0
2-80o 3-30o

-1 -2

KE(Nm)

CAM(Nm
)

o


2-80 3-30

1

o


2-80o 3-0o

100 50


2-80o 3-0o

0

2 Time(s)

(a)

4

0

0

2 Time(s)

4

(b)

Figure 12: Plots of (a) CAM and (b) KE of the biped robot while crossing a wide ditch with knee angle 80o and torso angles 30o 0o at μ = 0.6 and keeping the CoM with in the friction cone

14

0

0

0

0

0.5 1 X-Position(m)

0.5 1 X-Position(m)

0.5 1 X-Position(m)

0.5 1 X-Position(m)

1.5

1.5

1.5

1.5

0

0.5 1 X-Position(m)

0

0.5 1 X-Position(m)

(h) φ3 = 0◦ , μ = 0.3

-0.5

-0.5

0

0.5

1

1.5

(g) φ3 = 30◦ , μ = 0.3

1.5

-0.5

-0.5

-0.5

-0.5

0

0.5

1

1.5

-0.5

0

0

1.5

0.5

0.5

-0.5

1

1

-0.5

-0.5

1.5

0.5 1 X-Position(m)

1.5

0

0.5

1

1.5

1.5

0

(f) φ3 = 60◦ , μ = 0.3

-0.5

-0.5

0

0.5

1

1.5

e: φ3 = 90◦ , μ = 0.3

-0.5

-0.5

-0.5 1.5

0

0

0.5 1 X-Position(m)

0.5

0.5

0

1

1

-0.5

1.5

1.5

0

0

0

0

0.5 1 X-Position(m)

0.5 1 X-Position(m)

0.5 1 X-Position(m)

0.5 1 X-Position(m)

1.5

1.5

1.5

1.5

0

0.5 1 X-Position(m)

(l) φ3 = 0◦ , μ = 0.2

-0.5

-0.5

0

0.5

1

1.5

(k) φ3 = 30◦ , μ = 0.2

1.5

-0.5

-0.5

-0.5

-0.5

0

0.5

1

1.5

-0.5

0

0

1.5

0.5

0.5

0.5 1 X-Position(m)

1

1

-0.5

1.5

1.5

0

(j) φ3 = 60◦ , μ = 0.2

-0.5

-0.5 1.5

-0.5 0.5 1 X-Position(m)

0

0

0

0.5

0.5

-0.5

1

1.5

1

1.5

i: φ3 = 90◦ , μ = 0.2

-0.5

-0.5

-0.5 1.5

0

0

0.5 1 X-Position(m)

0.5

0.5

0

1

1

-0.5

1.5

1.5

0

0

0

0

0.5 1 X-Position(m)

0.5 1 X-Position(m)

0.5 1 X-Position(m)

0.5 1 X-Position(m)

1.5

1.5

1.5

1.5

Figure 13: Stick diagrams of the biped robot crossing a wide ditch with knee angle 80o and torso angle 90o , 60o , 30o and 0o at μ = 0.5,0.3 and 0.2

(d) φ3 = 0◦ , μ = 0.5

-0.5

-0.5

-0.5

0.5 1 X-Position(m)

0

0

0

0.5

0.5

-0.5

1

1.5

1

1.5

(c) φ3 = 30◦ , μ = 0.5

1.5

-0.5

-0.5

-0.5

0

0

1.5

0.5

0.5

0.5 1 X-Position(m)

1

1

-0.5

1.5

1.5

0

(b) φ3 = 60◦ , μ = 0.5

-0.5

-0.5

1.5

-0.5

0.5 1 X-Position(m)

0

0

0

0.5

0.5

-0.5

1

1.5

1

1.5

-0.5

-0.5

-0.5

1.5

0

0

0.5 1 X-Position(m)

0.5

0.5

0

1

1

-0.5

1.5

1.5

a: φ3 = 90◦ , μ = 0.5

Y-Position(m)

Y-Position(m)

Y-Position(m)

Y-Position(m)

Y-Position(m) Y-Position(m) Y-Position(m) Y-Position(m)

Y-Position(m) Y-Position(m) Y-Position(m) Y-Position(m)

15

Singularity analysis It can be observed that, as the torso angle decreases the front leg of the biped robot in Phase I, is stretched more. This stretch is due to the increased horizontal distance between the hip and the CoM at the starting point. Due to this when the swing foot contacts the ground at the end of phase I, the hip is still lagging behind causing the swing leg knee to stretch more. When the leg stretches completely it leads to singularity problems in finding the solution. To avoid singularity the initial drive force mentioned in Section 3.1 is increased. This increase will make the CoM and also the hip move faster, avoiding the necessity of stretching the leg more. The initial drive force is increased such that complete stretching of the front leg is avoided but further increase in this force can lead to singularity of the rear leg. In this paper we use two techniques to avoid knee stretch singularity. One is through changing the initial drive force and the other is by adjusting the swing foot motion trajectory. Having too small initial drive force takes more time for the CoM to cross the distance k4 . Too large force (within friction bound), while avoiding front leg knee singularity, can cause singularity in the rear leg knee. To limit the time for crossing the ditch, we fixed the minimum value of the drive force as 0.001mg. The initial drive force applied for various initial postures can be found from the plots given in Fig. 14. If changing the initial drive force does not avoid singularity of either the front knee or rear knee, we change the the foot height during swing to avoid singularity. For most of the cases, singularity free solutions for ditch crossing are obtained by changing the initial drive force alone, but for some cases it inevitable to change the foot motion in to avoid singularity as the initial drive force reaches its bounds. One such case can be observed in figure 13 (a), during phase II it can be observed that the foot trajectory is different from that of the other initial postures. In this case, initial drive force of 0.001mg (lower bound), while avoiding front leg singularity could not prevent rear leg singularity in phase II. Hence, we changed the rear foot trajectory by increasing the maximum foot height to 0.1 m which was 0.01 m for other cases. Another case where the singularity occurred at the front leg knee can be observed in Fig. 13(l) at μ = 0.2 and torso angle 0◦ . For this case drive force applied is equal to μmg (highest bound on initial drive force) as given in Fig. 14(c). But at lower friction coefficients this force alone, may not be sufficient to cross the ditch with out singularity. We got singularity free solutions for this case also by changing the foot trajectory. For some of the cases where the robot does not become singular but goes close to singularity, the power consumption of various joints increases. Stick diagrams and power consumption plots of the biped robot for such a case with initial knee angle 40 degrees and torso angle 30 degrees at μ equal to 0.5 are given in figure 15(a) and (b). In 15(a) the front leg of the robot stretches to the maximum and shrinks again by the end of phase I. At the end of phase II, torso is stretched more backwards due to which power consumption increases at the front hip and knee. In this case robot goes

towards singularity at both front leg and hip, due to this the net power consumption as a whole increases. Influence of varying knee angle Stick diagrams of the biped robot keeping torso angle 60◦ and varying knee angles at μ equal to 0.5 are given in Fig. 16. The knee angles considered are (a) 120◦ , (b) 100◦ , (c) 80◦ , and (d) 60◦ . It can be observed from the figure that when the knee angle is increased from 60 degrees to 120, the rear knee is bending towards the ditch. An increase in knee angle beyond 120 degrees is making the knee go into the ditch, which is not advisable with out knowing the shape of the ditch. It is always better to choose a knee angle below 120 degrees for crossing of a wide ditch. When the initial knee angle is decreased from 120 degrees to 60 degrees, the robot opens up at the knee and crossing a wide ditch at lower knee angles make the rear leg stretch completely leading to singularity as given figure 16(d). We could not get feasible solution with both of the techniques mentioned to avoid singularity.

16

Power consumption analysis The power consumption plots for various initial postures at coefficients of friction 0.5, 0.3, and 0.2 are given in Fig. 17(a) to (l). It can be observed from the figures that at any particular friction coefficient, most of the power is consumed in the middle region. In this region the CoM is outside the friction cone and the robot is applied with a maximum allowable force i.e., μmg once the CoM is outside the friction cone. Hence the joints consume most of the power in this region. The power consumed in the beginning and the end, though small for larger torso angles, gradually increase with the decrease in torso angle due to the increase in the initial drive force as given in Fig. 14. In Fig. 17, for a particular initial posture when coefficient friction varied from 0.5 to 0.2, the width of the middle region where the joints consume most of the power gradually increased. This increase is due to the reduction in the size of the friction cone. At lower torso angles (30◦ and 90◦ ), as μ is reduced power consumption of the joints gradually increases even in the beginning and at the end. This is because of the increase in the initial drive required as shown in Fig 14 (a), (b) and (c). This tells us that the robot has to share the load to all the joints in order to avoid slipping at lower torso angles and on low friction surfaces. The plots also reveal that much of the load is carried by the rear knee and hip joint in phase I. In Phase II the load is carried by both the front knee and the hip.

 3 -30o

 3 -60o

0.15 0.1 0.05 0 20

0.2

 3 -90o

 3 -0o

 3 -30o

 3 -60o

0.2

 3 -90o

0.15

 

 3 -0o

 

 

0.2

0.1 0.05

40

60

80

100

120

0.15

 3 -0o

 3 -30o

 3 -60o

 3 -90o

0.1 0.05

0 20

40


 



60

80

100

0 20

120

40


 



(a)

60

80

100

120


 



(b)

(c)

Figure 14: Initial Drive Force Coefficient (IDFC) of the biped robot with different initial postures at μ = (a) 0.5 (b) 0.3 (c) 0.2

1.5

1.5

1

1

0.5

0.5

0

0

Joint Power(W)

Y-Position(m)

1000

500

0 Rear knee Torso and rear leg Torso and front leg Front knee

-500

-0.5 -0.5

-0.5 0

0.5 1 X-Position(m)

1.5

-0.5

-1000 0

0.5 1 X-Position(m)

1.5

(a)

0

0.5

1

1.5 Time(s) (b)

2

2.5

3

Figure 15: Biped robot crossing a wide ditch with knee angle 40o and torso angle 30o at μ = 0.5 (a) Stick diagram (b) Power consumption at joints

17

1.5

1

1

0.5

0.5

0

0

-0.5 -0.5

Y-Position(m)

Y-Position(m)

1.5

-0.5 0

0.5 1 X-Position(m)

1.5

-0.5

1.5

1.5

1

1

0.5

0.5

0

0

-0.5 0

0.5 1 X-Position(m)

1.5

-0.5

-0.5 0

1.5

1.5

1

1

0.5

0.5

0

0

-0.5 -0.5

-0.5 0

0.5 1 X-Position(m)

1.5

-0.5

1.5

1.5

1.5

1

1

0.5

0.5

0

0

-0.5 0

0.5 1 X-Position(m)

-0.5

0

0.5 1 X-Position(m)

1.5

(b) θ2 = 100, μ = 0.5

Y-Position(m)

Y-Position(m)

(a) θ2 = 120, μ = 0.5

0.5 1 X-Position(m)

1.5

-0.5

(c) θ2 = 80, μ = 0.5

-0.5 0

0.5 1 X-Position(m)

1.5

-0.5

0

0.5 1 X-Position(m)

1.5

(d) θ2 = 60, μ = 0.5

Figure 16: Stick diagrams of the biped robot crossing a wide ditch with torso angle(φ3 )60o and knee angle(θ2 ) (a)120o (b)100o (c) 80o and (d) 60o at μ = 0.5

18

-1000

-500

0

500

1000

0

1

0

1

0

1

0

1

2

2

2

2

Time(s)

Time(s)

Time(s)

Time(s)

3

3

3

3

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

5

5

5

5

0

1

0

1

0

1

0

1

(h) φ3 = 0◦ , μ = 0.3

-1000

-500

0

500

1000

(g) φ3 = 30◦ , μ = 0.3

-1000

-500

0

500

1000

(f) φ3 = 60◦ , μ = 0.3

-1000

-500

0

500

1000

e: φ3 = 90◦ , μ = 0.3

-1000

-500

0

500

1000

2

2

2

2

Time(s)

Time(s)

Time(s)

Time(s)

3

3

3

3

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

5

5

5

5

0

1

0

1

0

1

0

1

(l) φ3 = 0◦ , μ = 0.2

-1000

-500

0

500

1000

(k) φ3 = 30◦ , μ = 0.2

-1000

-500

0

500

1000

(j) φ3 = 60◦ , μ = 0.2

-1000

-500

0

500

1000

i: φ3 = 90◦ , μ = 0.2

-1000

-500

0

500

1000

2

2

2

2

Time(s)

Time(s)

Time(s)

Time(s)

3

3

3

3

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

4

Rear knee Torso and rear leg Torso and front leg Front knee

5

5

5

5

Figure 17: Power consumption of various joints of the biped robot with knee angle 80◦ and torso angle 90◦ , 60◦ , 30◦ and 0◦ at μ = 0.5, 0.3 and 0.2

(d) φ3 = 0◦ , μ = 0.5

-1000

-500

0

500

1000

(c) φ3 = 30◦ , μ = 0.5

-1000

-500

0

500

1000

(b) φ3 = 60◦ , μ = 0.5

-1000

-500

0

500

1000

a: φ3 = 90◦ , μ = 0.5

Joint Power(W)

Joint Power(W)

Joint Power(W)

Joint Power(W)

Joint Power(W) Joint Power(W) Joint Power(W) Joint Power(W)

Joint Power(W) Joint Power(W) Joint Power(W) Joint Power(W)

19

various joints of the biped robot for different initial postures of the biped robot at coefficients of friction 0.5, 0.3, and 0.2 are given in Fig. 20. It can be seen that for higher torso angles the peak power requirement is more especially at μ equal to 0.5 than for 0.3 and 0.2. It was mentioned earlier that at higher torso angles the biped robot requires lower energy consumption to cross a wide ditch. For these cases net energy consumption is going to be almost same for coefficient frictions 0.5, 0.3, and 0.2. Hence both the criteria, least net energy consumption and least peak power consumption are satisfied for higher initial torso angles at low friction coefficient. We can apply the same solution at high friction coefficients also. In order to satisfy the criteria of both lower consumption and least peak power requirement at joints, we notice that the solution obtained for initial posture with knee angle 100 degrees and torso angle 90 degrees at μ of 0.3 is preferable. This solution can be applied for all the surfaces with coefficient of friction 0.3 or above. The peak power requirement Finding better initial postures of the biped robot and energy consumption are least of all for this posture, In order to find out better initial postures among the fea- however if we want to apply a common solution for a parsible initial postures, the net energy consumed by all the ticular initial posture at all coefficients of friction equal to joints together for crossing a ditch of width 1.05 m is calcu- or above 0.2, the preferable initial posture with lower energy lated. The net energy consumed is obtained by finding the consumption is knee angle 80 degrees and torso angle 90 dearea under net absolute power of all the joints over time. grees. But the peak power requirement is higher. Based on The plots of the net energy consumed for different initial the criteria of design one can choose the initial posture and postures at coefficients of friction 0.5, 0.3, and 0.2 are given design the actuation system using peak power data provided in Fig. 19 (a),(b), and (c). It can be observed that for a par- in the Fig. 20. ticular initial torso angle, the net energy consumption keeps increasing with increase in the initial knee angle. The rate Time analysis The time taken by the biped robot to of increase is low at high friction coefficient and is more at cross a wide ditch with various initial postures at differlow friction coefficients. However for some initial postures ent coefficients of friction are given in Fig. 21 along with the net energy consumption is high at lower knee angles the simulation time taken by MATLAB to run the algowhich is against the trend observed. This happens espe- rithm. The simulations were run on an intel-corei53210M cially for the initial postures that are on the boundary of CPU with a 2.5 GHz processor. It can be observed from the feasible regions given in Fig. 18. For these cases the the figure that the time taken by the biped robot decreases biped robot goes close to singularity but does not become with a decrease in torso angle. This is because the drive singular. One such case is discussed in Fig. 15. force applied increases with decrease in torso angle as given For a particular friction coefficient it can be observed Fig. 14 and the robot moves faster. It can also be observed that the lowest energy consumption for crossing a wide that the simulation time taken for a particular initial posditch occurs when the initial torso angle is 90 degrees. In ture is always close to or less than the time taken by the Fig. 19(a), (b) and (c) for many of the initial postures of robot to cross the ditch. Based on this we claim that the the biped robot solutions exist at all the friction coefficients proposed algorithm can give solutions in short time for onconsidered. The trajectory obtained for low friction surface line implementation on a biped robot. can be preferred for high friction surface provided the net energy consumption is less or same. In Fig. 19, for some 4.2.2 Active ankle joints initial postures with higher torso angles the total energy consumed is marginally same at all friction coefficients. We With active ankle joints ZMP is not going to stay at ancan apply these solutions to the same initial postures at kle. The location of ZMP is specified such that it is within higher friction coefficients also. So it is always preferable to the foot support polygon with control constraints. For the use higher torso angles in order to cross a wide ditch with biped robot to cross a ditch with feet and ZMP at a speciless energy consumption when friction coefficient is not well fied location, the length of the foot is considered as 0.27 m, with ankle at 0.04 m from heel as per specifications of the known. Another way to choose the best initial posture is based foot given in [30]. We also studied the influence of changing on the availability of actuators. Actuators with high power the height of CoM (yc ) while crossing a wide ditch. Stick are not preferable and some times they may not be available. diagrams of the biped robot with knee angle 80 degrees and It as always better to choose a case that requires less power torso angle 60 degrees at μ equal to 0.2, keeping the ZMP at rating of the actuators. The peak power requirements at 0.1 m from the ankle are given in Fig. 22. It is assumed that Finding the feasible regions With the proposed algorithm we made a detailed study with various initial postures and found an estimation for feasible region or the initial postures for which the robot can cross the given wide ditch at coefficients of friction 0.5, 0.3, and 0.2 and the feasible regions are given in Fig. 18. We have checked various initial postures in increments of 10◦ for torso angle and 20◦ for knee angles and drawn the feasible boundary. But the solution may or may not exist for every initial posture in between the feasible points given. We call the line in the Fig. 18 as approximate feasible boundary. We observed that, for initial postures on the left side, above the feasible boundary the biped robot becomes singular when crossing a wide ditch. In the region below and on the right side of the boundary line, either the biped robot knee goes below the ground level or joint angle limits of the biped robot are violated while crossing a wide ditch hence though the solutions exists they are not acceptable.

20

60 40 20 0 20

40

60

80


 



(a)

100

120

80

   


   


   


80

60 40 20 0 20

40

60

80

100

120

80 60 40 20 0

0


 



(b)

20

40

60

80

100

120


 



(c)

Figure 18: Feasible initial postures for a biped robot crossing a wide ditch at μ = (a) 0.5 (b) 0.3 (c) 0.2 origin is at the ankle of the stance foot. ZMP is specified at 0.1 m from the ankle in order to keep the ZMP close to the center of the foot support polygon that gives maximum stability margin. The dotted line in the figure indicates the maximum ankle to ankle distance in crossing the ditch and the solid line is the actual ditch. The width of the ditch is 0.78 m whereas the ankle to ankle distance is 1.05 m. The width of the ditch that the robot can cross decreased though the ankle to ankle distance does not change as the robot crosses the ditch keeping its stance foot completely outside the ditch. The figure also depicts the variation in yc as the robot crosses the ditch. The trajectory of yc given as a function of xc is shown in Fig. 23. As xc varies from 0.1 m to 0.625 m during phase-I and 0.625 m to 1.15 m during phase-II, yc moves from its initial position by 0.1 m below during phase I and attains its initial height again at the end of phase II. For the designed trajectory with displacement of yc (ycd ) as 0.1 m we can preverify whether vertical component of GRF is positive throughout the gait by solving the control constraints on xc and yc . The plot of y¨c is given in Fig. 24. The figure depicts that at any instant of time y¨c is always greater than the acceleration due to gravity (-9.81 sm2 ), thus ensuring the unilateral nature of vertical component of GRF. With yc moving down with respect to xc , CoM leaves the friction cone early, due to which the span of CoM outside the friction cone increases leading to an increase in CAM at the end of phase I. The plot of ZMP of the biped robot crossing the wide ditch for the same initial posture is given in Fig. 25. The shaded region indicates the region of foot support polygon. ZMP is at 0.1 m during phase I and it will shift to 1.15m during phase-II, hence the maximum dynamic stability of the biped ensured as the ZMP is at center of foot. We notice from Fig. 22 that by making the biped robot cross a ditch keeping the feet completely outside the ditch, the width of the ditch that the robot can cross reduced compared to that of crossing the ditch with point feet for the same ankle to ankle distance. In order to cross a wider ditch keeping the feet outside the ditch, the ankle to ankle distance at the end of the phase I has to be increased. Stick diagram of the biped robot crossing a ditch with maximum ankle to ankle distance of 1.27 m to cross a ditch of width 1 m are given in Fig. 26. ZMP and ycd are equal to 0.1 m. The analysis is done for a coefficient of friction of 0.2.

21

As we further increase the ankle to ankle distance although we could get the solutions but they are not valid as biped robot attains configurations which are not acceptable. Stick diagram of one such a case with maximum ankle to ankle distance of 1.32 m is given in Fig. 27. It can be seen in the figure that the robot has to lift its leg high during phase II in order to avoid singularity. Also during phase II shank crosses the foot, which is not acceptable for a biped robot with foot, however the solution can be implemented for a biped robot with point feet. In order to find out the influence of varying yc and ZMP, a comparison of the net energy and peak power consumption of the biped robot for the initial posture with knee angle 80◦ and torso angle 60◦ is given in Table 4, where k is a constant, which is the initial value of yc , MAAD is the maximum ankle to ankle distance and PPCJi is the peak power consumption of joint i. From the table it can be observed that crossing the ditch by moving yc down considerably increased the net energy consumption and peak power consumption at various joints, whereas changing ZMP location from ankle caused a slight increase in the net energy consumption due to ankle torques. The increase in the net energy consumption with yc motion is to attain the increased acceleration of yc during the end of phase I as shown in figure 24. Hence it is advisable to keep yc constant in order to efficiently cross a wide ditch. It is not advisable to move yc up in phase I as it increases the chances of biped robot becoming singular.

5

Conclusions • We have proposed an approach to obtain online solutions for crossing a wide ditch by a biped robot with passive and active ankle joints, considering friction, dynamic balance, and impact using the concept of control constraints. • Crossing a wide ditch by a biped robot is classified based on the location CoM with respect to friction cone and joint motions are obtained along with joint torques. • The task of ditch crossing has been studied thoroughly at coefficients of friction 0.6, 0.5, 0.3 and 0.2 for various initial postures of the biped robot. The peak power requirements at various joints and net energy

3-60 o

3-90 o

600 400 200 20

40

60

80

100

120

3-0o

800

3-30 o

3-60 o

3-90 o

Total Energy (Nm)

3-30 o

Total Energy (Nm)

Total Energy (Nm)

3-0o

800

600 400 200 20

40




60

80

100

120

3-0o

800

3-90 o

400 200 20

40

60

80

100

120


 




 


(a)

3-60 o

600





 


3-30 o

(b)

(c)

Figure 19: Net Energy consumption of the biped robot crossing a wide ditch with different initial postures at μ = (a) 0.5 (b) 0.3 (c) 0.2 yc = k and ZMP=0m MAAD(m) Net energy (Nm) PPCJ1(W) PPCJ2(W) PPCJ3(W) PPCJ4 (W) PPCJ5 (W) PPCJ6 (W)

1.05 398.90 0 361.5 327.1 602.3 104.6 0

1.27 832 0 608.9 1156.3 846.7 460.7 0

yc = k and ZMP=0.1m 1.05 427.96 53.1 343.3 264.3 626.5 91.7 53.4

1.27 881.85 63.1 615.8 1177.1 1018.3 206.5 78.5

yc =f(xc ) and ZMP=0m 1.05 480.2 0 422.7 1407.2 1960.8 158.7 0

1.27 970.6 0 847.3 3429 2843 536.3 0

yc =f(xc ) and ZMP=0.1m 1.05 515.55 155.3 703.5 943 1855.8 281.8 71.9

1.27 1135.4 89.8 676.5 2874.3 3000.6 514 215

Table 4: Influence of yc motion and ZMP location on net total energy and peak power consumption at the joints of the biped robot consumption of the biped robot are found to design the actuation system. We noticed that knee and hip joint actuators require more power rating.

length is small [31, 32]and is not efficient as the robot walks with bent knees [33, 34].

• Preferred initial postures considering the net energy consumption and peak power requirements at various joints were found. The net energy consumption is least if the robot starts keeping its torso upright. For a particular initial knee angle the net energy consumed by the robot reduces with increase in initial torso angle. • Foot trajectories have considerable influence on singularities that occur during the crossing. Different types of singularities that occur are presented and some techniques to avoid singularities have been discussed. • The biped robot requires less energy to cross a wide ditch on surfaces with high friction coefficient and requires more for low friction coefficients. However if the robot starts with an upright torso, the energy required did not vary much with change in friction coefficient.

• A comparison between simulation time and time for crossing are given for different initial postures and the simulation time is always lower than the time for crossing the wide ditch. Hence the solution obtained can be implemented online. In the present work, control constraints are formulated assuming that the landing position is known accurately. However, to implement this approach when the biped robot has to land with uncertainties or when the ground level difference on either side of the wide ditch is not known accurately due to measurement inaccuracies, the post impact velocities have to be found. The deviation in symmetry between phase I and phase II due to impact needs to be handled by an appropriately designed control constraints such that robot comes to complete rest. We will consider the case for future work.

• Active ankle joints require more energy than passive ankles.

The initial drive force has been chosen so as to arrive at singularity free solution. However, choice of the initial drive force needs to be rationalized or formulated to avoid trail and error procedure.

• Keeping the CoM height constant is more efficient than varying it. The result suggests that the energy optimality with respect to constant or varying height needs to be re-looked considering the stride length. For walking with a constant height of CoM, stride

The parameters which influence the motion of the robot are k3 , k4 , k2 , k5 etc. These parameters can change the joint angular velocities and torque requirement at a particular instant which will have a direct influence on the net energy consumption in crossing the ditch. All these parameters at present are chosen, 22

such that singularity is avoided. At the same time [7] the dynamic balance and Coulomb friction conditions also have to be satisfied. The values of these parameters chosen at present are not unique. Changing these [8] values changes the net energy consumption. Within the bounds of these parameters, different values can be chosen using optimization algorithm such that energy consumption is optimal. If the concern is to cross [9] the wide ditch with optimal energy starting with any initial posture, the analysis carried out in the present work has to be extended further with evolutionary al[10] gorithms.

SH Lam. On lagrangian dynamics and its control formulations. Applied Mathematics and Computation, 91(2):259–284, 1998. A Rosen. Applying the lagrange method to solve problems of control constraints. Journal of Applied Mechanics, 66(4):1013–1015, 1999. R Seifried and W Blajer. Analysis of servo-constraint problems for underactuated multibody systems. Mech. Sci, 4(1):113–129, 2013. Wojciech Blajer and Krzysztof Kolodziejczyk. A geometric approach to solving problems of control constraints: theory and a DAE framework. Multibody System Dynamics, 11(4):343–364, 2004.

As the peak power requirements for crossing a wide ditch are high, we feel that design of the biped robot with compliance for crossing a wide ditch may reduce the peak power requirements. In future we would like [11] Mihai Anitescu, Florian A Potra, and David E Stewto propose an approach for crossing wide ditch by a art. Time-stepping for three-dimensional rigid body biped robot with compliance. dynamics. Computer methods in applied mechanics and engineering, 177(3):183–197, 1999. Acknowledgments The authors wish to acknowledge the financial support received from the project Innovation Hub [12] Yildirim Hurmuzlu, Frank G´eNot, and Bernard Brogliato. Modeling, stability and control of biped on Cyber-Physical Systems, sponsored by CCBT, DeitY, robotsa general framework. Automatica, 40(10):1647– Ministry of Communications & IT, Government of India. 1664, 2004.

References [1]

[2]

[3]

[4]

[5]

[6]

[13] Hayder FN Al-shuka, Burkhard J Corves, Wen-hong Zhu, and Bram Vanderborght. A simple algorithm for generating stable biped walking patterns. International Byoung Soo Choi and S-M Song. Fully automated Journal of Computer Applications, 101(4), 2014. obstacle-crossing gaits for walking machines. Systems, Man and Cybernetics, IEEE Transactions on, [14] Mariano Garcia, Anindya Chatterjee, Andy Ruina, and 18(6):952–964, 1988. Michael Coleman. The simplest walking model: stability, complexity, and scaling. Journal of biomechanical Junshi Cheng and Junmin Pan. Ditch crossing conengineering, 120(2):281–288, 1998. trol for quadruped walking robot. In Intelligent Robots and Systems’ 93, IROS’93. Proceedings of the 1993 IEEE/RSJ International Conference on, volume 1, [15] Eric R Westervelt, Jessy W Grizzle, Christine Chevallereau, Jun Ho Choi, and Benjamin Morris. pages 537–541. IEEE, 1993. Feedback control of dynamic bipedal robot locomotion, volume 28. CRC press, 2007. A Fattah and A Fakhari. Trajectory planning of walking with different step lengths of a seven-link biped robot. In ASME 2010 International Design Engineer- [16] Xiuping Mu and Qiong Wu. Dynamic modeling and sliding mode control of a five-link biped during the ing Technical Conferences and Computers and Infordouble support phase. In American Control Confermation in Engineering Conference, pages 1361–1369. ence, 2004. Proceedings of the 2004, volume 3, pages American Society of Mechanical Engineers, 2010. 2609–2614. IEEE, 2004. Pandu Ranga Vundavilli and Dilip Kumar Pratihar. Dynamically balanced optimal gaits of a ditch-crossing [17] Hayder Al-Shuka, B Corves, and Wen-Hong Zhu. Dynamic modeling of biped robot using lagrangian and rebiped robot. Robotics and Autonomous Systems, cursive newton-euler formulations. International Jour58(4):349 – 361, 2010. nal of Computer Applications, 101:1–8, 2014. Wojciech Blajer. Dynamics and control of mechanical systems in partly specified motion. Journal of the [18] V Janardhan et al. Generating feasible solutions for dynamically crossing a wide ditch by a biped robot. Franklin Institute, 334(3):407–426, 1997. Journal of Intelligent & Robotic Systems, pages 1–20, 2017. A Rosen and E Edelstein. Investigation of a new formulation of the lagrange method for constrained dynamic systems. Journal of applied mechanics, 64(1):116–122, [19] Richard M Murray, Zexiang Li, S Shankar Sastry, and S Shankara Sastry. A mathematical introduction to 1997. robotic manipulation. CRC press, 1994.

23

[20] Parviz E Nikravesh. Planar Multibody Dynamics: Formulation, Programming and Applications. CRC press, 2007.

[33] Salman Faraji and Auke J Ijspeert. 3lp: a linear 3dwalking model including torso and swing dynamics. arXiv preprint arXiv:1605.03036, 2016.

[21] Spyros Tzafestas, Mark Raibert, and Costas Tzafes- [34] Hongbo Zhu, Minzhou Luo, Tao Mei, Jianghai Zhao, tas. Robust sliding-mode control applied to a 5-link Tao Li, and Fayong Guo. Energy-efficient bio-inspired biped robot. Journal of Intelligent and Robotic Sysgait planning and control for biped robot based on hutems, 15(1):67–133, 1996. man locomotion analysis. Journal of Bionic Engineering, 13(2):271–282, 2016. [22] Christine Azevedo, Philippe Poignet, and Bernard Espiau. On line optimal control for biped robots. In Proceedings of the IFAC 15th World Congress, 2002. [23] Miomir Vukobratovi´c and Branislav Borovac. Zeromoment pointthirty five years of its life. International Journal of Humanoid Robotics, 1(01):157–173, 2004. [24] Ching-Long Shih. Ascending and descending stairs for a biped robot. Systems, Man and Cybernetics, Part A: Systems and Humans, IEEE Transactions on, 29(3):255–268, 1999. [25] R Caballero and M Armada. Zero moment point modeling using harmonic balance. In Climbing and Walking Robots, pages 689–699. Springer, 2005. [26] Shuuji Kajita, Kenji Kaneko, Kensuke Harada, Fumio Kanehiro, Kiyoshi Fujiwara, and Hirohisa Hirukawa. Biped walking on a low friction floor. In Intelligent Robots and Systems, 2004.(IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference on, volume 4, pages 3546–3552. IEEE, 2004. [27] Bruno Siciliano and Oussama Khatib. Springer handbook of robotics. Springer, 2016. [28] Qiang Huang, Kazuhito Yokoi, Shuuji Kajita, Kenji Kaneko, Hirohiko Arai, Noriho Koyachi, and Kazuo Tanie. Planning walking patterns for a biped robot. Robotics and Automation, IEEE Transactions on, 17(3):280–289, 2001. [29] Ambarish Goswami and Vinutha Kallem. Rate of change of angular momentum and balance maintenance of biped robots. In Robotics and Automation, 2004. Proceedings. ICRA’04. 2004 IEEE International Conference on, volume 4, pages 3785–3790. IEEE, 2004. [30] Aydin T¨ozeren. Human body dynamics: classical mechanics and human movement. Springer Science & Business Media, 2000. [31] Young-Dae Hong, Chang-Soo Park, and Jong-Hwan Kim. Stable bipedal walking with a vertical centerof-mass motion by an evolutionary optimized central pattern generator. IEEE Transactions on Industrial Electronics, 61(5):2346–2355, 2014. [32] Ren C Luo, Hong-Hao Chang, Jun Sheng, and PengHsi Chang. Walking pattern generation with nonconstant body height biped walking robot. In Industrial Electronics Society, IECON 2013-39th Annual Conference of the IEEE, pages 4318–4323. IEEE, 2013. 24

3-0o

3-30 o

3-60 o

Rear Hip (Nm/s)

Rear Knee (Nm/s)

1500

3-90 o

1000 500 0 20

40

60

80

100

120

1500

3-0o

3-30 o

3-60 o

3-90 o

1000 500 0 20

40

60

80

100

3-90 o

0 20

40

60

80

100

120


 



120

Front Knee (Nm/s)

Front Hip (Nm/s)

3-0o

3-60 o

500


 

1500

3-30 o

1000

1500

3-0o

3-30 o

3-60 o

3-90 o

1000 500 0 20

40


 



60

80

100

120


 



1500

3-0o

3-30 o

3-60 o

Rear Hip (Nm/s)

Rear Knee (Nm/s)

(a)

3-90 o

1000 500 0 20

40

60

80

100

120

1500

3-0o

3-30 o

3-60 o

3-90 o

1000 500 0 20

40

60

80

100

3-90 o

500 0 20

40

60

80

100

120


 



120

Front Knee (Nm/s)

Front Hip (Nm/s)

3-0o

3-60 o

1000


 

1500

3-30 o

1500

3-0o

3-30 o

3-60 o

3-90 o

1000 500 0 20

40




60

80

100

120


 




 


1500

3-0o

3-30 o

3-60 o

Rear Hip (Nm/s)

Rear Knee (Nm/s)

(b)

3-90 o

1000 500 0 20

40

60

80

100

120

1500

3-0o

3-30 o

3-60 o

3-90 o

1000 500 0 20

40

60

80

100

3-90 o

500 0 20

40

60

80

100

120


 



120

Front Knee (Nm/s)

Front Hip (Nm/s)

3-0o

3-60 o

1000


 

1500

3-30 o


 



1500

3-0o

3-30 o

3-60 o

3-90 o

1000 500 0 20

40

60

80

100

120


 



(c)

Figure 20: Peak power consumption of biped robot joints for crossing a wide ditch with different initial postures at μ = (a) 0.5 (b) 0.3 (c) 0.2

25

3-30 o

3-60 o

3-90 o

4 2 20

40

60

80

100

120

5 4

3-0o

Simulation Time(s)

Simulation Time(s)

6 3-60 o

3-90 o

4

2 20

40

60

80

3-90 o

2 20

40

60

80

100

120

4 3-0o

3

100

120


 
 5 4

3-0o

3-30 o

3-60 o

3-90 o

3 2 20

40




60

80

100

120

1 20

3-90 o

40

60

80

100

120

4 3-0o

3

3-30 o

3-60 o

3-90 o

2 1 20

40

60

80

100

120


 




 


(a)

3-60 o


 







 


3-30 o

2





 
 3-30 o

3-60 o

3




3-0o

3-30 o

Simulation Time(s)

3-0o

Ditch crossing Time(s)

Ditch crossing Time(s)

Ditch crossing Time(s)

6

(b)

(c)

Figure 21: Plot of time for crossing the ditch vs simulation time with different initial postures at μ = (a) 0.5 (b) 0.3 (c) 0.2

8

1

1

0.5

0.5

0

0

-0.5

-0.5

-0.5

0

0.5 1 X-Position(m)

1.5

-0.5

6

 

1.5

Y-Position(m)

1.5

4 2 0 -2

0

0.5 1 X-Position(m)

1.5

0

0.5

1

1.5 Time(s)

2

2.5

3

Figure 22: Stick diagram of biped robot crossing the ditch Figure 24: Plot of acceleration of yc for the biped robot keeping the ZMP at 0.1m from the ankle and maximum crossing a ditch with initial posture, θ2 = 80o and φ3 = 60o , ankle to ankle distance 1.05m with θ2 = 800 and φ3 = 600 at μ = 0.2

0.95 Zero Moment Point (m)

1.5




0.9

0.85

0.8

0

0.2

0.4

0.6



0.8

1

0.5 0 -0.5

1.2

Figure 23: Plot of yc as a function of xc for the biped robot crossing a ditch with initial posture, θ2 = 80o and φ3 = 60o , at μ = 0.2

1

0

0.5

1

1.5 Time(s)

2

2.5

3

Figure 25: Plot of ZMP for the biped robot crossing a ditch with varying yc and initial posture, θ2 = 80o and φ3 = 60o , at μ = 0.2

26

Y-Position(m)

1.5

1.5

1

1

0.5

0.5

0

0

-0.5 -0.5

-0.5 0

0.5 1 X-Position(m)

1.5

-0.5

0

0.5 1 X-Position(m)

1.5

Y-Position(m)

Figure 26: Stick diagram of biped robot crossing the ditch keeping the ZMP at 0.1m from the ankle and maximum ankle to ankle distance 1.27m with θ2 = 800 and φ3 = 600 at μ =0.2

1.5

1.5

1

1

0.5

0.5

0

0

-0.5

-0.5

-0.5

0

0.5 1 X-Position(m)

1.5

-0.5

0

0.5 1 X-Position(m)

1.5

Figure 27: Stick diagram of biped robot crossing the ditch keeping the ZMP at 0.1m from the ankle and maximum ankle to ankle distance 1.32m with θ2 = 800 and φ3 = 600 at μ =0.2

27

Appendix A

Converting control constraints from body to joint coordinate formulation

The body coordinates of the biped robot are represented by the vector c and the joint coordinates are represented by the vector θ as given below. ⎡ ⎤ x1 ⎢ y1 ⎥ ⎢ ⎥ ⎢ φ1 ⎥ ⎢ ⎥ ⎢ x2 ⎥ ⎢ ⎥ ⎢ y2 ⎥ ⎢ ⎥ ⎡ ⎤ ⎢ φ2 ⎥ θ1 ⎥ ⎢ ⎢ θ2 ⎥ ⎢ x3 ⎥ ⎢ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ and θ=⎢ (57) c = ⎢ y3 ⎥ ⎢ θ3 ⎥ ⎢ φ3 ⎥ ⎣ θ4 ⎦ ⎥ ⎢ ⎢ x4 ⎥ θ5 ⎥ ⎢ ⎢ y4 ⎥ ⎢ ⎥ ⎢ φ4 ⎥ ⎥ ⎢ ⎢ x5 ⎥ ⎥ ⎢ ⎣ y5 ⎦ φ5

˙ For a biped robot with 5 dof of the The relation between velocities of body and joint coordinates are given as c˙ = B θ. present case the relationship is given in equation A. The the 15X5 matrix in the middle is B matrix for this case. The horizontal and vertical components of the foot position and CoM in body coordinates are given in equations 59,60,61 and 62. The complete derivation for converting the equations 35 and 36 into joint coordinates is given in 63. ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

x˙ 1 y˙ 1 φ˙ 1 x˙ 2 y˙ 2 φ˙ 2 x˙ 3 y˙ 3 φ˙ 3 x˙ 4 y˙ 4 φ˙ 4 x˙ 5 y˙ 5 φ˙ 5

⎤

⎡

− l12s1

l1 c 1 ⎥ ⎢ ⎥ ⎢ 2 ⎥ ⎢ 1 ⎥ ⎢ ⎥ ⎢ − l22s2 − l1 s1 ⎥ ⎢ ⎥ ⎢ l2 c 2 ⎥ ⎢ 2 + l1 c 1 ⎥ ⎢ 1 ⎥ ⎢ ⎥ ⎢ l 3 s3 s − l −l ⎥ ⎢ 2 2 1 s1 − 2 ⎥ ⎢ l2 c2 + l1 c1 + l32c3 ⎥=⎢ ⎥ ⎢ 1 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ − l42s4 − l2 s2 − l1 s1 ⎥ ⎢ l4 c 4 ⎥ ⎢ 2 + l2 c 2 + l1 c 1 ⎥ ⎢ ⎥ ⎢ 1 ⎥ ⎢ ⎥ ⎢ −l4 s4 − l2 s2 − l1 s1 − l5 s5 2 ⎥ ⎢ ⎦ ⎣ l4 c 4 + l 2 c 2 + l 1 c 1 + l5 c 5 2 1 

0 0 0 − l22s2 l2 c 2 2

1 −l2 s2 − l32s3 l2 c2 + l32c3 1 − l42s4 − l2 s2 l4 c 4 2 + l2 c 2 1 −l4 s4 − l2 s2 − l52s5 l4 c4 + l2 c2 + l52c5 1 

0 0 0 0 0 0 − l32s3 l3 c 3 2

0 0 0 0 0 0 0 0 0

1 − l42s4

− l42s4

1 −l4 s4 − l52s5 l4 c4 + l52c5 1

1 −l4 s4 − l52s5 l4 c4 + l52c5 1

l4 c 4 2

l4 c 4 2

0 0 0 0 0 0 0 0 0 0 0 0 − l52s5 l5 c 5 2

1

B

where si = sin φi = sin(θ1 + θ2 + . . . + θi ) and ci = cos φi = cos(θ1 + θ2 + . . . + θi ) where i = 1 . . . 5 l5 cos φ5 2 l5 yf 1 = y5 + sin φ5 2 m1 x1 + m2 x2 + m3 x3 + m4 x4 + m5 x5 xc = m m 1 y 1 + m 2 y 2 + m3 y 3 + m4 y 4 + m 5 y 5 yc = m l5 x˙ f 1 = x˙ 5 − sin φ5 φ˙ 5 2 l5 y˙ f 1 = y˙ 5 + cos φ5 φ˙ 5 2 28

xf 1 = x5 +

⎤

⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎡ ⎥ ⎥ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

θ˙1 θ˙2 θ˙3 θ˙4 θ˙5

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(58)



(59) (60) (61) (62)

⎤ ⎡ x˙ f 1 0 ⎢ y˙ f 1 ⎥ ⎢ 0 ⎢ ⎢ ⎥ 1 ⎣ x˙ c ⎦ = ⎢ ⎣ m m y˙ c 0  ⎡

x˙ f 1 = D1 c˙

0 0 0 m1 m

0 0 0 0

0 0 m2 m

0

0 0 0 m2 m

0 0 0 0

0 0 m3 m

0

0 0 0 m3 m

0 0 0 0 

0 0 m4 m

0

0 0 0 m4 m

0 0 0 0

1 0 m5 m

0

0 1 0 m5 m

− l25 sin φ5 l5 2 cos φ5 0 0

D

y˙ f 1 = D2 c˙

and

⎤

⎥ ⎥ ⎥ c˙ ⎦ 

th

where Di is the i row of the matrix D, similarly x˙ c = D3 c˙ and y˙ c = D4 c˙ ¨ + D˙ 1 c˙ ¨ + D˙ 3 c˙ and x ¨ c = D3 c x ¨ f 1 = D1 c ¨ + D˙ 2 c˙ ¨ + D˙ 4 c˙ and y¨c = D4 c y¨f 2 = D2 c ¨ = B θ¨ + B˙ θ˙ substituting c˙ = B θ˙ and c ˙ + D˙ 1 B θ˙ = D1 B θ¨ + (D1 B˙ + D˙ 1 B)θ˙ x ¨f 1 = D1 (B θ¨ + B˙ θ) ˙ + D˙ 2 B θ˙ = D2 B θ¨ + (D2 B˙ + D˙ 2 B)θ˙ y¨f 1 = D2 (B θ¨ + B˙ θ) ˙ + D˙ 3 B θ˙ = D3 B θ¨ + (D3 B˙ + D˙ 3 B)θ˙ x ¨c = D3 (B θ¨ + B˙ θ) ˙ + D˙ 4 B θ˙ = D4 B θ¨ + (D4 B˙ + D˙ 4 B)θ˙ y¨c = D4 (B θ¨ + B˙ θ) ˙ x ¨c = Jxc θ¨ + Fxc (θ, θ) ˙ y¨c = Jyc θ¨ + Fyc (θ, θ) Constraints on foot motion

where

Jxc = D3 B

and

where

Jyc = D4 B

and

Fxc = (D3 B˙ + D˙ 3 B)θ˙ Fyc = (D4 B˙ + D˙ 4 B)θ˙

Foot motion constraints are given in equations (35) and (36)

¨c into equation (35) we get substituting x ¨f 1 and x   2 (D1 − α1 D3 )B θ¨ + ((D1 B˙ + D˙ 1 B) − α1 (D3 B˙ + D˙ 3 B))θ˙ = (f1 (θc ) − 2f1 (θc ) tan θc )θ˙c

  2 (D1 − α1 D3 )B θ¨ = (f1 (θc ) − 2f1 (θc ) tan θc )θ˙c − ((D1 B˙ + D˙ 1 B) − α1 (D3 B˙ + D˙ 3 B))θ˙ ˙ θc , θ˙c ) Jx1 θ¨ = Fp1 (θ, θ,

(63)

where 



2





2

Fp1 = (f1 (θc ) − 2f1 (θc ) tan θc )θ˙c − ((D1 B˙ + D˙ 1 B) − α1 (D3 B˙ + D˙ 3 B))θ˙ Jx1 = (D1 − α1 D3 )B Similarly equation (36) can be converted to the form ˙ θc , θ˙c ) Jy1 θ¨ = Gp1 (θ, θ,

(64)

Gp1 = (g1 (θc ) − 2g1 (θc ) tan θc )θ˙c − ((D2 B˙ + D˙ 2 B) − β1 (D3 B˙ + D˙ 3 B))θ˙ Jy1 = (D2 − β1 D3 )B Following the similar steps, when y˙ c = 0 and y¨c = 0 Jx1 = (D1 − α1 D3 + α2 D4 )B

 2 2f1 (θc )y˙ c θ˙c ˙ − ((D1 B˙ + D˙ 1 B) − α1 (D3 B˙ + D˙ 3 B) + α2 (D4 B˙ + D˙ 4 B))θ˙ Fp1 = (f1 (θc ) − 2f1 (θc ) tan θc )θc − yc Jy1 = (D2 − β1 D3 + β2 D4 )B    2 2g1 (θc )y˙ c θ˙c ˙ − ((D2 B˙ + D˙ 2 B) − β1 (D3 B˙ + D˙ 3 B) + β2 (D4 B˙ + D˙ 4 B))θ˙ Gp1 = (g1 (θc ) − 2g1 (θc ) tan θc )θc − yc 



Constraint on the motion of horizontal CoM(xc )

The constraint on motion of xc in acceleration form is given as

x ¨c − kf y¨c = kf mg ˙ − kf (Jyc θ¨ + Fyc (θ, θ)) ˙ = kf g ⇒ (Jxc θ¨ + Fxc (θ, θ))

˙ − kf Fyc (θ, θ)) ˙ ⇒ (Jxc − kf (Jyc )θ¨ = kf g − (Fxc (θ, θ))

Constraint on the motion of vertical CoM(yc )

(65)

The constraint on motion of yc in acceleration form is given as

y¨c − fy (xc )¨ xc = fy (xc )x˙ 2c ˙ − f  (xc )(Jxc θ¨ + Fxc (θ, θ)) ˙ = f  (xc )x˙ 2 ⇒ (Jyc θ¨ + Fyc (θ, θ)) y y c 292    ¨ ˙ ˙ ⇒ (Jyc − fy (xc )Jxc )θ = fy (xc )x˙ c − (Fyc (θ, θ) − fy (xc )Fxc (θ, θ))

(66)

Constraint on ZMP Constraint on ZMP is a nonholonomic constraint and is given as   ¨i + mi (xi − Zr )¨ yi + Ii φ¨i = mi g(xi + Zr )i = 1 . . . 5 −mi yi x  ¨= ⇒ Dz c mi g(xi + Zr ) where

  Dz = −m1 y1 m1 (x1 − Zr ) I1 − m2 y2 m2 (x2 − Zr ) I2 . . . − m5 y5 m5 (x5 − Zr ) I5  ˙ = mi g(Zr − xi ) ⇒ Dz (B θ¨ + B˙ θ)

⇒ Dz B θ¨ = mg(Zr − xc ) − Dz B˙ θ˙ Jz θ¨ = Fz

(67)

where Jz = D z B

Fz = mg(Zr − xc ) − Dz B˙ θ˙

Constraint Jacobian matrix (J) Combining all the five constraint equations from (63) to (67) we get ⎡ ⎤ ⎤ ⎡ ˙ θc , θ˙c ) Fp1 (θ, θ, Jx1 ⎥ ⎢ ⎥ ⎢ ˙ θc , θ˙c ) Gp1 (θ, θ, Jy1 ⎢ ⎥ ⎥ ⎢ ⎥¨ ⎢ ⎢ ⎥ ˙ − kf Fyc (θ, θ)) ˙ ⎥ ⎢ Jxc − kf Jyc ⎥ θ = ⎢ kf g − (Fxc (θ, θ)) ⎥ ⎢  ⎢  ˙ − f  (xc )Fxc (θ, θ)) ˙ ⎥ ⎣ fy (xc )x˙ 2c − (Fyc (θ, θ) ⎦ ⎣ Jyc − fy (xc )Jxc ⎦ y Jz Fz       J

RHS

The first four constraints are holonomic and the fifth constraint is nonholonomic. In equation (10) ⎡ ⎡ ⎤ ⎤ ˙ θc , θ˙c ) Fp1 (θ, θ, Jx1 ⎢ ⎢ ⎥ ⎥ ˙ θc , θ˙c ) Gp1 (θ, θ, Jy1 ⎢ ⎢ ⎥ ⎥ Fh = ⎢ Jh = ⎢ Jn = J z ⎥ ⎥ ˙ − kf Fyc (θ, θ)) ˙ kf g − (Fxc (θ, θ)) ⎣ Jxc − kf Jyc ⎦ ⎣ ⎦  ˙ − f  (xc )Fxc (θ, θ)) ˙ Jyc − fy (xc )Jxc fy (xc )x˙ 2c − (Fyc (θ, θ) y

30

Fn = F z

(68)


 
"
!

!
 
 
 '
 #
$ !"'(
!"
!
 
 

!""#"

'
 !)
# "'

!

" 
!"#"
"

!""#"



'
' )
!
# "
! 
" !"!
 

"!

#$"
)


!"
#
"
!

!
 
 

"!% 
$ !"'(
!"
!

" 
 
 

!""#"

'
 # )

!
# "'
% 
!
!!"
 !!

"
 ""



 !
 
"

!""#"

'
' )
!
# "
! 
" !"!
 
"
"!(
&

#" !(

#"'
'!)