Modeling and Control of Humanoid in Dynamic Environments: iCub Balancing on a Seesaw

Gabriele Nava, Daniele Pucci, Nuno Guedelha, Silvio Traversaro, Francesco Romano, Stefano Dafarra and Francesco Nori1

Abstract— Forthcoming applications concerning humanoid robots may involve physical interaction between the and a dynamic environment. In such scenario, classical balancing and walking controllers that neglect the environment dynamics may not be sufficient for achieving a stable robot behavior. This paper presents a modeling and control framework for balancing humanoid robots in contact with a dynamic environment. We first model the robot and environment dynamics, together with the contact constraints. Then, a control strategy for stabilizing the full system is proposed. Theoretical results are verified in simulation with robot iCub balancing on a seesaw.

I.INTRODUCTION Prospective applications for may require robots to step out on protected and well-known workspaces and phys- Fig. 1. iCub balancing on a seesaw. ically interact with dynamic, human-centered environments. In this context, a is required to balance, walk, perform manipulation tasks and – even more important dynamics. However, the control problem is complicated – safely interact with humans. by system’s underactuation, that forbids full state feedback The importance of controlling the robot interaction with linearization [11]. the environment calls for the design of torque and impedance At the control level, an efficient algorithm for balancing control algorithms, capable of exploiting the forces the robot and walking of humanoid robots is the so-called momentum- exerts at contact locations, for performing balancing and based control [5], which often exploits prioritized stack-of- walking tasks [1], [2], [3]. However, the applicability of tasks. In particular, the primary control objective is the stabi- such controllers in a real scenario is often limited by the lization of centroidal momentum dynamics [12]. Momentum assumption that the robot is in contact with a rigid, static control can be achieved by properly choosing the contact environment. From the modeling point of view, this results in forces the robot exerts at contact locations [2], [13], [14]. neglecting the environment dynamics, i.e. the robot is subject Robot joint torques are then used for generating the desired to a set of purely kinematic constraints [4]. This assumption forces. To get rid of the (eventual) actuation redundancy may be a limitation in case the robot is walking on debris or associated with momentum control, a lower priority task is uneven ground. Different solutions that make use of adaptive usually added during the stabilization of the robot momen- or robust controllers are available in literature [5], [6], [7], tum, whose main role is the stabilization of the so-called [8]. There are also situations in which the environment robot zero dynamics [15]. In this paper, we propose a modeling and control frame- arXiv:1707.09226v2 [cs.RO] 9 Mar 2018 dynamics may be known a priori, e.g. interacting with a wheeled chair, balancing on a moving platform or even work for balancing a humanoid robot on a seesaw board interacting with humans. This leads to the development of (Fig.1). Similar problems have been already addressed in control strategies that try to stabilize both the robot and the literature [9], [8]. In particular, in [9] the authors developed contact dynamics [4], [9]. a torque control strategy based on weighted control policies, On the modeling side, the fixed base assumption may together with online and offline model adaptations, for bal- be a strong limitation for a humanoid robot, capable – at ancing a humanoid robot on a moving platform. We follow least theoretically– to move from place to place without a similar approach, but we then design a different control being physically attached to the ground. The floating base algorithm. In particular, the aim of our paper is to apply a formalism [10], i.e. none of the robot links has an a priori momentum-based control strategy in case of balancing in a constant pose with respect to an inertial reference frame, dynamic environment. is particularly well suited for modeling humanoid robots The remaining of the paper is organized as follows. SectionII recalls notation, robot modeling and a momentum- *This paper was supported by the FP7 EU project CoDyCo (No. 600716 based control strategy for balancing with rigid, static con- ICT 2011.2.1 Cognitive Systems and Robotics) 1 All authors belong to the iCub Facility department, Istituto Italiano di tacts. Sections III–IV detail the modeling and control frame- Tecnologia, Via Morego 30, , Italy [email protected] work designed for balancing in dynamic environments. Sim- ulation results on humanoid robot iCub are presented in 5], it is possible to apply a coordinate transformation in the SectionV. Conclusions and perspectives conclude the paper. state space (q, ν) that transforms the system dynamics (1) into a new form where the mass matrix is block diagonal, II.BACKGROUND thus decoupling joint and base frame accelerations. Also, In this section, we provide a description of the modeling in this new set of coordinates, the first six rows of Eq. (1) and control framework developed in [14], [16] for balancing are the centroidal dynamics1. As an abuse of notation, we a humanoid robot in a rigid environment. assume that system (1) has been transformed into this new set of coordinates, i.e. A. Notation   • We denote with I the inertial frame of reference, whose Mb(q) 06×n M(q) = ,H = MbvC, (2) z axis points against the gravity, and with S a frame 0n×6 Mj(q) attached to the seesaw board, whose origin coincides 6×6 n×n : > > > 6 with the seesaw center of mass. with Mb ∈ R ,Mj ∈ R , H = (HL ,Hω ) ∈ R the robot centroidal momentum, and H ,H ∈ 3 the linear • We make use of the subscript s to distinguish the seesaw L ω R dynamic and kinematic quantities. and angular momentum at the center of mass, respectively. The new base frame velocity is denoted by v ∈ 6, which • The constant g denotes the norm of the gravitational C R in the new coordinates yielding a block-diagonal mass matrix acceleration. The constants m and ms represent the 3 is given by vC = (p ˙c, ωo), where p˙c ∈ R is the velocity of masses of the robot and the seesaw, respectively. 3 3 3×3 the system’s center of mass p ∈ , and ω ∈ is the • We denote with S(x) ∈ R the skew-symmetric c R o R matrix such that S(x)y = x × y, where × indicates so-called system’s average angular velocity. 3 Lastly, we assume that the location where a contact occurs the cross product operator in R . m×n † n×m on a link remains constant w.r.t. the inertial frame, i.e. the • Given a matrix A ∈ R , we denote with A ∈ R its Moore Penrose pseudoinverse. system is subject to a set of holonomic constraints of the m form: JC (q)ν = 0. The constraints equations associated • ei ∈ R is the canonical vector, consisting of all zeros k but the i-th component that is equal to one. with all the rigid contacts can be represented as   B. Modeling robot dynamics JC1 (q) J(q)ν = ··· ν = J(q)ν = 0, (3) A robot is usually modeled as a set of n + 1 rigid bodies,   JC (q) namely links, connected by n joints with one degree of nc freedom each. We assume that the robot has no links with h i> where J = J > ... J > ∈ 6nc×(n+6) is the con- a priori fixed position and orientation with respect to the C1 Cnc R inertial frame of reference, i.e. it is a free floating system. straints jacobian. By differentiating the kinematic constraint Because of the above assumption, an element of the robot (3), one obtains I I ˙ configuration space can be defined as q = ( pB, RB, qj) Jν˙ + Jν = 0. (4) and belongs to the Lie group Q = R3 × SO(3) × Rn. It is composed by the position and orientation of a base frame In view of (1), the equations of motion along the con- B attached to a robot link w.r.t. the inertial reference frame, straints (4) are given by: and the joints positions qj. By differentiating q we obtain > I I M(q)ν ˙ + h(q, ν) = Bτ + J(q) f (5a) the expression of system’s velocities ν = ( p˙B, ωB, q˙j) = 3 3 n s.t. (vB, q˙j) ∈ R × R × R , where the angular velocity of the I I I I base frame ωB is chosen such that R˙ B = S( ωB) RB. Jν˙ + Jν˙ = 0. (5b) It is assumed the robot is exerting nc distinct wrenches (n+6) on the environment. Applying the Euler-Poincare´ formalism where h := C(q, ν)ν + G(q) ∈ R , while f = 6nc as in [17, Ch. 13.5] results in the following equations of (f1, ··· , fnc ) ∈ R are the set of contact wrenches – i.e. motion: Lagrange multipliers – making Eq. (4) satisfied. n Xc M(q)ν ˙ + C(q, ν)ν + G(q) = Bτ + J > f C. Balancing control on static contacts Ck k (1) k=1 We recall now the torque control strategy developed in where M ∈ R(n+6)×(n+6) is the mass matrix, C ∈ previous publications [14], [16] for balancing a humanoid R(n+6)×(n+6) accounts for Coriolis and centrifugal effects, robot with static contacts. It is a task-based control with two (n+6) > G ∈ R represents the gravity term, B = (0n×6, 1n) tasks: the task with higher priority is the control of robot is a selector of the actuated degrees of freedom, τ ∈ Rn is a centroidal momentum, while the secondary task is to stabilize vector representing the internal actuation torques, and fk ∈ the system’s zero dynamics. R6 denotes an external wrench applied by the environment 1In the specialized literature, the term centroidal dynamics is used to to the link of the k-th contact. The Jacobian JC = JC (q) is k k indicate the rate of change of the robot’s momentum expressed at the center- the map between the system’s velocity ν and the linear and of-mass, which then equals the summation of all external wrenches acting angular velocity at the k-th contact. As described in [18, Sec. on the multi-body system [12]. First, observe that the contact constraints equations Eq. (4) Lemma 1 ([20]). Assume that Λ is full row rank. Then, the instantaneously relate the contact wrenches f with the con- closed loop joint space dynamics does not depend upon the trol input, namely the joint torques τ. In fact, by substituting wrench redundancy f0. the state accelerations from Eq. (5a) into Eq. (4), one has: This result is a consequence of the postural control JM −1(J >f − h + Bτ) + Jν˙ = 0. (6) choice (11) and it is of some interest: it means that the closed loop joint dynamics depends on the total rate-of- Writing explicitly the control torques from Eq. (6) gives: change of the momentum, i.e. H˙ , but not on the different † −1 > τ = Λ (JM (h − J f) − Jν˙ ) + NΛτ0 (7) forces generating it. Hence, any choice of the redundancy f0 does not influence the joint dynamics, and we can exploit it −1 6nc×n n×n with Λ = JM B ∈ R ; NΛ ∈ R is the projector to minimize the joint torques τ in Eq. (7). n onto the nullspace of Λ, and τ0 ∈ R is a free variable. If In the language of Optimization Theory, we can rewrite matrix Λ has rank greater than the dimension of f, one can the control strategy as the following optimization problem: use τ to generate any desired contact wrenches f ∗ by means ∗ ∗ 2 of Eq. (7). f = argmin |τ (f)| (12a) f s.t. 1) Momentum control: recall now that the rate-of-change of the robot momentum equals the net external wrenches Af < b (12b) acting on the robot, which in the present case reduces to the H˙ (f) = H˙ ∗ (12c) ∗ 2 contact wrenches f plus the gravity wrench: τ (f) = argmin |τ − τ0(f)| (12d) τ ˙ > H(f) = Jb f − mge3, (8) s.t. ˙ where H ∈ R6 is the robot momentum, while because of the J(q, ν)ν + J(q)ν ˙ = 0 (12e) 6nc×6 −1 > transformation applied in Eq. (2) matrix Jb ∈ R can be ν˙ = M (Bτ + J f−h) (12f)   obtained by partitioning the contact Jacobian J = Jb Jj . > τ0 = hj − J f + u0. (12g) In view of Eq. (8), and assuming that the contact wrenches j can be chosen at will, we can choose f such that: The constraints (12b) ensure the satisfaction of friction cones, normal contact surface forces, and center-of-pressure ˙ ˙ ∗ ˙ d ˜ H(f) = H := H − KpH − KiIH˜ (9) constraints. The control torques are then given by τ=τ ∗(f ∗). d ˜ d where H is the desired robot momentum, H = H − H III.MODELINGENVIRONMENTDYNAMICS is the momentum error and K ,K ∈ 6×6 two symmetric, p i R The closed loop system (1)–(7)–(10)–(11) exploits the positive definite matrices. The integral of robot momentum assumption that the location where a contact occurs on a link error, I is obtained as described in [19]. H˜ remains constant w.r.t. the inertial frame, i.e. Jν = 0. There Observe that in case n > 1 (e.g. balancing on two feet), c are situations, however, in which the environment dynamics there are infinite sets of contact wrenches that satisfy Eq. (9). cannot be neglected. In this case, Eq. (4) becomes: We parametrize the set of solutions f ∗ as: ˙ ∗ Jν˙ + Jν = af . f = fH + Nbf0 (10) 6nc   where af ∈ R represents the accelerations at the contact > † ˙ ∗ 6nc×6nc with fH = (Jb ) H + mge3 , Nb ∈ R the locations. Our case study exemplifies this last situation: the > 6nc projector into the null space of Jb , and f0 ∈ R the robot is balancing with both feet leaning on a seesaw board of wrench redundancy that does not influence H˙ (f) = H˙ ∗. semi-cylindrical shape (Fig.1). In what follows, we present a Then, the control torques that instantaneously realize the modeling and control framework derived from (1)–(7)–(10)– contact wrenches f ∗ are given by Eq. (7): (11) for two feet balancing on a seesaw. ∗ † −1 > ∗ τ = Λ (JM (h − J f ) − Jν˙ ) + NΛτ0 A. Seesaw dynamics The seesaw can be considered a single rigid body with no 2) Stability of the Zero Dynamics: we are now left to a priori fixed position and orientation w.r.t. the inertial frame. define the free variable τ , that may be used to ensure the 0 We also assume the seesaw is in contact with both the robot stability of the so called zero dynamics of the system [15]. A feet and a rigid ground, exerting on them the reaction forces choice of τ that ensures the stability of the zero dynamics 0 and moments −f and the contact forces and moments f , in case of one foot balancing is [19]: s respectively. A complete description of the seesaw dynamics > τ0 = hj − Jj f + u0 (11) is given by the equations representing the rate of change of seesaw momentum, i.e. H˙ s. In particular, we project the : j d j where u0 = −KpNΛMj(qj − qj ) − KdNΛMjq˙j, and rate of change of seesaw momentum in the seesaw frame S j n×n j n×n Kp ∈ R and Kd ∈ R two symmetric, positive previously defined, resulting in the following equations of definite matrices. An interesting property of the closed loop motion: system (1)–(7)–(10)–(11) is recalled in the following Lemma. S > > Ms ν˙s + hs = − Jr f + Js fs (13) 6×6 S 6 where Ms ∈ R is the seesaw mass matrix, ν˙s ∈ R is obtaining Eq. (18) is more general, and can be reused in case the vector of seesaw linear and angular accelerations, hs ∈ the robot is interacting with different dynamic environments. 6 represents the Coriolis and gravity terms. The jacobians R IV. THEBALANCINGCONTROLSTRATEGY Jr and Js are the map between the seesaw velocity in the S Given the effectiveness of control law (7)–(10)–(11) for seesaw frame νs and the velocities at the contacts locations. In particular, in the chosen representation the mass matrix balancing in a rigid environment, it may worth trying to extend this framework to the case of balancing on a seesaw. Ms is given by: " # First, we make use of Eq. (17) to relate the feet wrenches f ms13 03 with the contact forces and moments fs. By substituting Eq. Ms = S 03 Is (13) into (17), one has:

S 3×3 J M −1(J >f − h − J >f) + J˙ ν = 0. (19) and matrix Is ∈ R is constant, thus simplifying the s s s s s r s s formulation of seesaw dynamics. Further details on the Writing explicitly fs from Eq. (19) gives: derivation of Eq. (13) can be found in the Appendix VII- −1 −1 > ˙ A. As an abuse of notation but for the sake of clarity let us fs = Γ (JsMs (hs + Jr f) − Jsνs), S omit from now on the superscript S, e.g. νs = νs. −1 > with Γ = (JsMs Js ) . Now, substitute the above equation B. Modeling contact constraints into Eq. (13): ¯ It is assumed that the robot feet are always attached to the Msν˙s + hs = Asf (20) seesaw, resulting in the following set of constraints: ¯ > −1 −1 > ˙ where hs = (16 − Js Γ JsMs )hs + Js Jsνs, while the νfeet = Jrνs = Jν. (14) matrix multiplying the feet forces and moments is given by A = −(1 − J >Γ−1J M −1)J >. It is worth noting that Note that the above equation is coupling the seesaw and the s 6 s s s r matrix A is not full rank, but instead rank(A ) = 1. This robot dynamics. By differentiating Eq. (14), one has: s s is not surprising, in fact Eq. (17) implies the seesaw can Jν˙ + Jν˙ = Jrν˙s + J˙rνs. (15) only roll, and therefore it has only 1 degree of freedom left. Assuming the constraints Eq. (17) are always satisfied Eq. (15) will substitute Eq. (4) in the formulation of the for any (reasonable) value of fs, it means that the seesaw system’s equations of motion. Lastly, we define the contact dynamics can be stabilized by controlling, for example, P point as the intersection between the contact line of the the seesaw angular momentum dynamics along the lateral seesaw with the ground and its frontal plane of symmetry. direction, and this can be done by using the feet wrenches We assume that the seesaw is only rolling, and this implies as a fictitious control input of Eq. (20). Hence, one may think P v = 0 that the linear velocity of the contact point is p . of controlling the whole system dynamics by means of the Furthermore, let the seesaw frame be oriented as in figure following control objectives: 1. The shape of the (semi-cylindrical) seesaw constrains the • control of the robot momentum together with the seesaw rotation along the y and z axis. We model all the above angular momentum along the lateral direction by means mentioned constraints as follows: of feet wrenches f; Jsνs = 0, (16) • ensure the stability of the system’s zero dynamics as before by exploiting joint torques redundancy. and differentiating Eq. (16) gives: Concerning the primary control objective, the matrix project- ˙ Jsν˙s + Jsνs = 0. (17) ing f in the robot momentum and seesaw angular momentum

6nc×6 5×6 equations can be obtained from Eq. (8) and Eq. (20) and it is: The shape of Jr ∈ R , Js ∈ R and their derivatives > h > i is described in the Appendix VII-B. Finally, the system Af = Jb As e4 . However, we performed a numerical dynamics for the robot balancing on a seesaw is given by analysis on matrix Af for different state configurations, the following set of equations: by means of singular value decomposition (SVD) method. Numerical results point out that matrix A is not full rank, floating base dynamics f but instead rank(A ) = 6. This implies that does not always > s Mν˙ + h = Bτ + J f (18a) exist a set f ∗ of feet wrenches that can generate any desired seesaw dynamics trajectory for the given primary control task. > > Msν˙s + hs = −Jr f + Js fs (18b) 1) Control of robot momentum only: being not possible to constraint: feet attached to the seesaw always control both the seesaw and the robot momentum dy- ˙ ˙ Jν˙ + Jν = Jrν˙s + Jrνs (18c) namics, we first decide to control only the robot momentum constraint: seesaw is rolling as primary task, and then verify numerically that the seesaw angle trajectory still remains bounded within a limited range. Jsν˙s + J˙sνs = 0. (18d) For the given task, f ∗ is the same of Eq. (10): The above equations are valid for the specific case of a ∗ > †  ˙ ∗  semi-cylindrical seesaw, but the approach we followed for f = (Jb ) H + mge3 + Nbf0 2) Control of mixed momentum: another possibility is 3) Joint torques and zero dynamics: it is now possible to trying to control a quantity which depends on both the robot apply the same procedure presented inII for relating the joint and the seesaw dynamics, for example the rate of change of torques τ with the desired feet wrenches f ∗. By substituting system’s momentum: the state accelerations from Eq. (18a)–(20) into Eq. (18c), and writing explicitly the control torques one has: H˙ = J >f − (m + m)ge = J >A f + f (21) t t s s 3 t t bias † ¯ τ = Λ (h − Jf¯ − Jν˙ + J˙rνs) + NΛτ0 (23) where Eq. (21) is obtained as described in Appendix VII- ¯ −1 −1 −1 > with h = JM h − JrM hs, and J¯ = JM J − 5×6 s C. However, matrix Jt ∈ : hence, the maximum rank −1 R JrMs As. The redundancy of joint torques is used for of matrix J >A is 5, and again it is not possible to always t t stabilizing the robot posture, and therefore τ0 remains the ensure the convergence of the total momentum to any desired same of Eq. (11). The wrench redundancy f0 is still used trajectory by means of f. To overcome this problem, we for minimizing the joint torques. As before, we can rewrite decide to control the linear momentum of the robot only, the control strategy as an optimization problem: together with the angular momentum of the whole system. ∗ ∗ 2 The motivation behind this choice is not only to have a full f = argmin |τ (f)| (24a) rank matrix multiplying the feet wrenches, but it also exploits f s.t. the difference of magnitude between the robot mass (31Kg) and the seesaw mass (4Kg), which implies the center of mass Af < b (24b) of the overall system is actually close to the center of mass H˙ (f) = H˙ ∗ (24c) of the robot. ∗ 2 τ (f) = argmin |τ − τ0(f)| (24d) Consider now the following partitions of robot and total τ momentum: s.t. ˙ ˙     Jν + Jν˙ = Jrνs + Jrν˙s (24e) HL HtL H := ,Ht := J˙sνs + Jsν˙s = 0 (24f) Hω Htω ν˙ = M −1(Bτ + J >f−h) (24g) H ,H ∈ 3 −1 > > where L ω R are the linear and angular momentum ν˙s = Ms (Js fs − Jr f−hs) (24h) 3 of the robot, whereas HtL,Htω ∈ are the system linear > R τ0 = hj − J f + u0. (24i) and angular momentum. We define the mixed momentum as j  > Hm = HL Htω . Being H˙ m = H˙ m(f), we can choose where in case the primary control objective is the stabiliza- f such that: tion of the mixed momentum trajectories, Eq. (24c) is of the ˙ ˙ ∗ form: Hm(f) = Hm. ∗ d H˙ := H˙ − KpH˜m − KiI ˜ (22) m m Hm V. SIMULATION RESULTS d ˜ We test the control solutions proposed in Section III by where Hm is the desired mixed momentum, Hm = Hm − d 6×6 using a model of the humanoid robot iCub [21] with 23 Hm is the momentum error and Kp,Ki ∈ R two symmetric, positive definite matrices. It is in general not degrees-of-freedom (DoFs). possible to define an analytical expression for the integral A. Simulation Environment of the angular momentum [12], and it is not straightforward to extend the results we presented in [19] to this new case. Simulations are performed by means of a Simulink con- troller interfacing with Gazebo simulator [22]. The controller Therefore, I ˜ is of the following form: Hm frequency is 100 [Hz]. Of the different physic engines that " # can be used with Gazebo, we chose the Open Dynamics m(x − xd) I = c c Engine (ODE). Furthermore, Gazebo integrates the dynamics H˜m 03,1 with a fixed step semi-implicit Euler integration scheme. The advantage of using this simulation setup is twofold. 3 where xc ∈ R is the robot center of mass position. The feet First, we only have to specify the model of the robot, ∗ wrenches f that instantaneously realize the desired mixed and the constraints arise naturally while simulating. Another momentum rate of change are then given by: advantage of using Gazebo consists in the ability to test directly on the real robot the same control software used  ! ∗ † ˙ ∗ SLmge3 in simulation. f = Am Hm + + Nmf0 −Sωfbias B. Robustness to external disturbances     where SL = 13 03 , Sω = 03 13 are selec- We first perform a robustness test on the closed loop tor matrices; the multiplier of feet wrenches is Am = system (18)–(24). The control objective is to stabilize the h i> system about an equilibrium position. After 20 [s] an external J S> A>J S> and N is a null space projector. b L t t ω m force of amplitude 100 [N] is applied to the robot torso along the lateral direction for a period of 0.01 [s]. Figure 10 0.1 Robot Momentum Control Robot Mom. Control Mixed Momentum Control Mixed Mom. Control 8 Reference 0.05

6 0 External force applied 4

-0.05 2

0 -0.1 0 10 20 30 40 50 0 10 20 30 40 50

Fig. 2. Norm of robot linear momentum error while balancing. Fig. 5. Lateral component of robot CoM position. Both controllers are able to track the desired CoM trajectory, but with mixed momentum control is possible to achieve better results. 1.5 Robot Mom. Control: |H - H d| Mixed Mom. Control: |H - H d | orientation, even if not explicitly controlled. t t 1 C. Tracking Performances

External force applied We then evaluate the two control laws for tracking a 0.5 desired trajectory of the robot center of mass. The reference trajectory for the center of mass is a sinusoidal curve with amplitude of 2.5 [cm] and frequency of 0.25 [Hz] along 0 the robot lateral direction. A dedicated gain tuning has been 0 10 20 30 40 50 performed on both controllers in order to achieve better results. It is important to point out that the main scope of Fig. 3. Norm of robot and system angular momentum error while balancing. this analysis is not a comparison between the controllers performances, but rather to verify the stability of the closed loop system for the given task. Figure5 shows the lateral 10 Robot Mom. Control component of the robot center of mass position during the Mixed Mom. Control tracking. The black line is the reference trajectory. The red 5 line is the center of mass position obtained with the mixed momentum control, and the blue line is obtained with the 0 robot momentum control. Both controllers are able to track the desired trajectory. However, with the mixed momentum -5 control is possible to achieve better results, while the other External force applied control strategy depicts poor tracking even after gain tuning.

-10 A possible explanation is that in order to keep balancing on 0 10 20 30 40 50 the seesaw while tracking the CoM trajectory, the robot may be required to have an angular momentum reference different Fig. 4. Seesaw orientation. Even if not explicitly controlled, its trajectory from zero. Trying to regulate the robot angular momentum remains bounded while the robot is balancing. to zero may worsen the tracking performances.

VI.CONCLUSIONS 2 shows the norm of robot linear momentum error for This paper proposes a modeling and control framework both control laws. Analogously, Figure3 depicts the norm for balancing a humanoid robot in dynamic environments. of robot and system angular momentum error when the In particular, the case study is the robot balancing on a primary task is to control the robot momentum and the mixed seesaw board. The system equations of motion and the momentum, respectively. After the external force is applied, constraints are obtained following a general procedure, that both controllers are still able to bring the system back to the can be reused in case the robot is in contact with a different equilibrium position. Figure4 shows instead the behaviour object, or even a human. The control algorithm is redesigned of the seesaw orientation θ. The blue line represent θ when taking into account the seesaw dynamics, and two different the control objective is to stabilize the robot momentum, controllers are proposed. While both controllers show a good while the red line is when the primary task is to control the response to external disturbances, the mixed momentum mixed momentum. In both cases, the trajectory of θ remains control shows better tracking performances. bounded even after the application of the external force, and In this paper, no experimental results are presented. How- therefore both controllers are able to stabilize also the seesaw , preliminary tests on the real robot iCub have been performed, depicting some limitations of our control ap- The constraint of having the feet attached to the seesaw I I I proach. For example, on real applications modeling errors implies that ωlf = ωrf = ωs. Also, one has: and network delays can strongly affect the performances. I v = I v − S(I p )I ω Also, a proper estimation of parameters such as the seesaw lf s sl s I I I I orientation and angular velocity plays a very important role vrf = vs − S( psr) ωs I I in the effectiveness of our control strategies. In order to where psl, psr represent the distance between the seesaw be able to move on the real robot, future works might be CoM and the left and right foot, respectively. Then, focused on reducing the modeling and estimation errors,  I  together with the design of a control law capable of dealing 13 −S( psl) 0 1  with model uncertainties, for example by means of adaptive I ν =  3 3  I ν = J¯ I ν f 1 −S(I p ) s r s control algorithms.  3 sr  03 13 VII.APPENDIX Analogously, the constraint of only rolling implies that the A. Seesaw dynamics in frame S velocity at the contact point P between the seesaw and the I I I I Let us define with S[I] a reference frame whose origin is ground is given by: vp = vs − S( psp) ωs = 0, thus I I I I at the seesaw center of mass, and with the orientation of the implying vs = S( psp) ωs. The variable psp represents inertial frame I. The rate of change of seesaw momentum, the distance between the seesaw CoM and the contact point when projected in this frame, is given by: P . Also, constraining the rotation along y and z axis implies S[I] ˙ ¯> ¯> the second and third component of the seesaw angular Hs = − msge3 − Jr f + Js fs (25) >I >I velocity are given by: e2 ωs = e3 ωs = 0. Then, one has: where the matrices J¯r and J¯s are defined in the next  I  subsection VII-B. Note that the mapping between frame S[I] 13 −S( psp) and the seesaw frame S is given by the relative rotation > I ¯ I 01,3 e2  νs = Js νs = 0 I  >  between the inertial frame and the seesaw frame, i.e. Rs. 01,3 e S[I] 3 Therefore, the projection of the seesaw momentum Hs ¯ ¯ into the seesaw frame S is given by: The matrices Js and Jr are then obtained from Js and Jr as described in the previous subsection. "I # S[I] I ¯ S Rs 03 S Hs = Rs Hs = I Hs. (26) C. Total momentum rate of change 03 Rs The system linear and angular momentum is obtained as By differentiating Eq. (26), one has: a combination of the robot and seesaw momentum: S[I] I ˙ S I S t ∗ t ∗ S[I] H˙ s = R¯s Hs + R¯s H˙ s. (27) Ht = Xc H + XS[I] Hs. (29) S S For not burdening the notation, we dropped the superscripts Recall that Hs = Ms νs, and that the derivative of a rotation matrix is given by: I R˙ = I R S(S ω ), being S ω denoting the frames w.r.t. Ht and H are expressed. The s s s s t ∗ the angular velocity of the seesaw projected in the seesaw transformation matrices in the space of wrenches Xc and tX∗ are of the following form: frame. Also, in the seesaw frame the mass matrix Ms is S[I] constant. Therefore, one has: " # t ∗ 13 03 S[I] I S S S Xx = I I H˙ s = R¯s(S¯( ωs)Ms νs + Ms ν˙s). (28) S( px − pt) 13 ¯ S I I I I with S( ωs) a proper block diagonal matrix Finally, by where px = pc for the robot momentum and px = pS[I] substituting Eq. (28) into (25), and multiplying both sides by for the seesaw momentum. Then, the derivative of Eq. (29) I ¯−1 I ¯> Rs = Rs , one is left with Eq. (13), where we define: is given by: ∗ ∗ I ˙ t ˙ t ˙ S[I] t ∗ ˙ t ∗ S[I] ˙ Jr = J¯r R¯s Ht = Xc H + XS[I] Hs + Xc H + XS[I] Hs. ¯ I ¯ Js = Js Rs Recall that the system’s center of mass position is related S S I > I h = S¯( ω )M ν + R¯ m ge to the robot and seesaw center of mass as follows: pt = s s s s s s 3 I I m pc+ms pS[I] m+m . Also, recall that S(x)x = 0. Then, it is B. Derivation of matrices J and J s ∗ ∗ r s t ˙ t ˙ S[I] possible to verify that Xc H + XS[I] Hs = 0, and Recall the vector of feet linear and angular velocities therefore the rate of change of system’s momentum is: expressed in the inertial frame: ˙ t ∗ ˙ t ∗ S[I] ˙ Ht = Xc H + XS[I] Hs. (30)  I  vlf I By substituting now Eq. (8),(25) into Eq. (30), one has:  ωlf  I ν =   . ∗ f I v  H˙ = tX (J >f − mge ) + (31)  rf  t c b 3 I ω t ∗ ¯> ¯> rf XS[I](−msge3 − Jr f + Js fs). > ¯> Observe that Jb and Jr , being the transformations mapping [15] A. Isidori, “The zero dynamics of a nonlinear system: From the the wrenches from the contact locations to the seesaw and origin to the latest progresses of a long successful story,” European > c ∗ c ∗ Journal of Control, vol. 19, no. 5, pp. 369 – 378, 2013, the Path of robot center of mass, are of the form: Jb = Xl Xr Control. [Online]. Available: http://www.sciencedirect.com/science/ h ∗ ∗i and J¯> = S[I]X S[I]X . Thus f is simplified from article/pii/S0947358013000836 r l r [16] D. Pucci, F. Romano, S. Traversaro, and F. Nori, “Highly dynamic I Eq. (30). Furthermore, one can verify that S( pS[I] − balancing via force control,” in 2016 IEEE-RAS 16th International I I I Conference on Humanoid Robots (Humanoids), Nov 2016, pp. 141– pt)msge3 + S( pc − pt)mge3 = 0. This is consistent ˙ 141. with the definition of Ht as the summation of all external [17] J. E. Marsden and T. S. Ratiu, Introduction to Mechanics and Symme- wrenches, i.e the contact forces fs and the gravity wrench. try: A Basic Exposition of Classical Mechanical Systems. Springer Publishing Company, Incorporated, 2010. By substituting fs into Eq. (30) by means of (19), we finally [18] S. Traversaro, D. Pucci, and F. Nori, “A unified view of the obtain Eq. (21), where we define: equations of motion used for control design of humanoid robots,” Submitted, 2017. [Online]. Available: https://traversaro.github.io/ > t ∗ ¯> Jt = XS[I]Js preprints/changebase.pdf −1 −1 > [19] G. Nava, F. Romano, F. Nori, and D. Pucci, “Stability analysis and At = Γ JsMs Jr design of momentum-based controllers for humanoid robots,” Pro- −1 −1 ceedings of the 2016 IEEE/RSJ International Conference on Intelligent fbias = Γ (JsM hs − J˙sνs) − (ms + m)ge3. s Robots and Systems IROS, Oct 2016. [20] D. Pucci, G. Nava, and F. Nori, “Automatic gain tuning of a mo- mentum based balancing controller for humanoid robots,” in 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Hu- REFERENCES manoids), Nov 2016, pp. 158–164. [21] G. Metta, L. Natale, F. Nori, G. Sandini, D. Vernon, L. Fadiga, C. von [1] C. Ott, M. A. Roa, and G. Hirzinger, “Posture and balance control Hofsten, K. Rosander, M. Lopes, J. Santos-Victor, A. Bernardino, and for biped robots based on contact force optimization,” in 2011 11th L. Montesano, “The iCub humanoid robot: An open-systems platform IEEE-RAS International Conference on Humanoid Robots, Oct 2011, for research in cognitive development,” Neural Networks, vol. 23, no. pp. 26–33. 89, pp. 1125 – 1134, 2010, social Cognition: From Babies to Robots. [2] B. Stephens and C. Atkeson, “Dynamic balance force control for [22] N. Koenig and A. Howard, “Design and use paradigms for gazebo, compliant humanoid robots,” in Intelligent Robots and Systems (IROS), an open-source multi-robot simulator,” Intelligent Robots and Systems, 2010 IEEE/RSJ International Conference on, Oct 2010, pp. 1248– 2004. (IROS 2004). Proceedings. 2004 IEEE/RSJ International Con- 1255. ference on, pp. 2149 – 2154, 2004. [3] E. Farnioli, M. Gabiccini, and A. Bicchi, “Optimal contact force distribution for compliant humanoid robots in whole-body loco- manipulation tasks,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), May 2015, pp. 5675–5681. [4] A. D. Luca and C. Manes, “Modeling of robots in contact with a dy- namic environment,” IEEE Transactions on Robotics and Automation, vol. 10, no. 4, pp. 542–548, Aug 1994. [5] S.-H. Lee and A. Goswami, “A momentum-based balance controller for humanoid robots on non-level and non-stationary ground,” Autonomous Robots, vol. 33, no. 4, pp. 399–414, 2012. [Online]. Available: http://dx.doi.org/10.1007/s10514-012-9294-z [6] J.-Y. Kim, I.-W. Park, and J.-H. Oh, “Walking control algorithm of biped humanoid robot on uneven and inclined floor,” Journal of Intelligent and Robotic Systems, vol. 48, no. 4, pp. 457–484, Apr 2007. [Online]. Available: https://doi.org/10.1007/s10846-006-9107-8 [7] M. Morisawa, S. Kajita, F. Kanehiro, K. Kaneko, K. Miura, and K. Yokoi, “Balance control based on capture point error compensation for biped walking on uneven terrain,” in 2012 12th IEEE-RAS Inter- national Conference on Humanoid Robots (Humanoids 2012), Nov 2012, pp. 734–740. [8] S. H. Hyon, “Compliant terrain adaptation for biped humanoids with- out measuring ground surface and contact forces,” IEEE Transactions on Robotics, vol. 25, no. 1, pp. 171–178, Feb 2009. [9] S. O. Anderson and J. K. Hodgins, “Adaptive torque-based control of a humanoid robot on an unstable platform,” in 2010 10th IEEE- RAS International Conference on Humanoid Robots, Dec 2010, pp. 511–517. [10] R. Featherstone, Rigid Body Dynamics Algorithms. Secaucus, NJ, USA: Springer-Verlag New York, Inc., 2007. [11] J. A. Acosta and M. Lopez-Martinez, “Constructive feedback lineariza- tion of underactuated mechanical systems with 2-DOF,” Decision and Control, 2005 and 2005 European Control Conference. CDC-ECC ’05. 44th IEEE Conference on, 2005. [12] D. Orin, A. Goswami, and S.-H. Lee, “Centroidal dynamics of a humanoid robot,” Autonomous Robots, 2013. [13] A. Herzog, L. Righetti, F. Grimminger, P. Pastor, and S. Schaal, “Bal- ancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics,” in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on, Sept 2014, pp. 981–988. [14] F. Nori, S. Traversaro, J. Eljaik, F. Romano, A. Del Prete, and D. Pucci, “iCub whole-body control through force regulation on rigid noncoplanar contacts,” Frontiers in Robotics and AI, vol. 2, no. 6, 2015.