arxiv: v4 [math.oc] 16 Jul 2017

Transcription

1 Staility Analysis and Design of Momentum-ased Controllers for Humanoid Roots Gariele Nava, Francesco Romano, Francesco Nori and Daniele Pucci arxiv:63.478v4 math.oc 6 Jul 27 Astract Envisioned applications for humanoid roots call for the design of alancing and walking controllers. While promising results have een recently achieved, roust and reliale controllers are still a challenge for the control community dealing with humanoid rootics. Momentum-ased strategies have proven their effectiveness for controlling humanoids alancing, ut the staility analysis of these controllers is still missing. The contriution of this paper is twofold. First, we numerically show that the application of state-of-the-art momentum-ased control strategies may lead to unstale zero dynamics. Secondly, we propose simple modifications to the control architecture that avoid instailities at the zero-dynamics level. Asymptotic staility of the closed loop system is shown y means of a Lyapunov analysis on the linearized system s joint space. The theoretical results are validated with oth simulations and experiments on the icu humanoid root. I. INTRODUCTION Humanoid rootics is doutless an emerging field of engineering. One of the reasons accounting for this interest is the need of conceiving systems that can operate in places where humans are foridden to access. To this purpose, the scientific community has paid much attention to endowing roots with two main capailities: locomotion and manipulation. At the present day, the results shown at the DARPA Rootics Challenge are promising, ut still far from eing fully reliale in real applications. Staility issues of oth low and high level controllers for alancing and walking were among the main contriutors to root failures. A common highlevel control strategy adopted during the competition was that of regulating the root s momentum, which is usually referred to as momentum-ased control. This paper presents numerical evidence that momentum-ased controllers may lead to unstale zero dynamics and proposes a modification to this control scheme that ensures asymptotic staility. Balancing controllers for humanoid roots have long attracted the attention of the rootic community, 2. Kinematic and dynamic controllers have een common approaches for ensuring a stale root ehavior for years 3,4. The common denominator of these strategies is considering the root attached to ground, which allows one for the application of classical algorithms developed for fixed-ased manipulators. At the modeling level, the emergence of floating-ase formalisms for characterizing the dynamics of multi-ody systems has loosened the assumption of having a root *This paper was supported y the FP7 EU project KoroiBot (No. 699 ICT 23. Cognitive Systems and Rootics) The authors are with the icu Facility department, Istituto Italiano di Tecnologia, Via Morego 3, Genoa, Italy name.surname@iit.it link attached to ground 5. At the control level, instead, one of the major complexities when dealing with floating ase systems comes from the root s underactuation. In fact, the underactuation forids the full feedack linearization of the underlying system 6. The lack of actuation is usually circumvented y means of rigid contacts etween the root and the environment, ut this requires close attention to the forces the root exerts at the contact locations. If not regulated appropriately, uncontrolled contact forces may reak the contact, and the root control ecomes critical 7,8. Contact forces/torques, which act on the system as external wrenches, have a direct impact on the rate-of-change of the root s momentum. Indeed, the time derivative of the root s momentum equals the net wrench acting on the system. Furthermore, since the root s linear momentum can e expressed in terms of the center-of-mass velocity, controlling the root s momentum is particularly tempting for ensuring oth root and contact staility (see, e.g., 9 for proper definition of contact staility). Several momentum-ased control strategies have een implemented in real applications,,2. The essence of these strategies is that of controlling the root s momentum while guaranteeing stale zero-dynamics. The latter ojective is often achieved y means of a postural task, which usually acts in the null space of the control of the root s momentum 3,4,5. These two tasks are achieved y monitoring the contact wrenches, which are ensured to elong to the associated feasile domains y resorting to quadratic programming (QP) solvers 7,8,6. The control of the zero-dynamics can also e used to control the root joint configuration 5. The control of the linear momentum is exploited to stailize a desired position for the root s center of mass. In contrast, the choice of desired values for the root angular momentum is still unclear 7. Hence, control strategies that neglect the control of the root s angular momentum have also een implemented 8. Controlling oth the root linear and angular momentum, however, is particularly useful for determining the contact torques, and it has ecome a common torque-controlled strategy for dealing with alancing and walking humanoids. Controlling also the angular momentum results in a more human-like ehavior and etter response to perturations 9,2. To the est of the authors knowledge, the staility analysis of momentum-ased control strategies in the contexts of floating ase systems is still missing. The contriution of this paper goes along this direction y considering a humanoid root standing on one foot, and is then twofold. First, we

2 present numerical evidence that classical momentum-ased control strategies may lead to unstale zero dynamics, thus meaning that classical postural tasks are not sufficient in these cases. The main cause of this instaility is the lack of orientation correction terms at the angular momentum level: we show that correction terms of the form of angular momentum integrals are sufficient for ensuring asymptotic staility of the closed loop system, which is proved y means of a Lyapunov analysis. The postural control, however, is modified with respect to (w.r.t.) state-of-the-art choices. The validity of the presented controller is tested oth in simulation and on the humanoid root icu. This paper is organized as follows. Section II introduces the notation, the system modeling, and also recalls a classical momentum-ased control strategy. Section III presents numerical results showing that momentum ased control strategies for humanoid roots may lead to unstale zero dynamics. Section IV presents a modification of the momentum ased control strategy for which staility and convergence can e proven. Section V discusses the numerical and experimental validation of the proposed approach. Conclusions and perspectives conclude the paper. A. Notation II. BACKGROUND I denotes an inertial frame, with its z axis pointing against the gravity. The constant g denotes the norm of the gravitational acceleration. e i R m is the canonical vector, consisting of all zeros ut the i-th component that is equal to one. Given two orientation frames A and B, and two vectors A p, B p R 3 expressed in these orientation frames, the rotation matrix A R B is such that A p = A R B B p. Let S(x) R 3 3 e the skew-symmetric matrix such that S(x)y = x y, where is the cross product operator in R 3. Given a function f(x, y) : R n R m R p, the partial derivative of f( ) w.r.t. the variale x is denoted as x f(x, y) = f(x,y) x R p n. B. Modelling It is assumed that the root is composed of n + rigid odies, called links, connected y n joints with one degree of freedom each. We also assume that the multi-ody system is free floating, i.e. none of the links has an a priori constant pose with respect to the inertial frame. The root configuration space can then e characterized y the position and the orientation of a frame attached to a root s link, called ase frame B, and the joint configurations. Thus, the configuration space is defined y Q = R 3 SO(3) R n. An element of Q is then a triplet q = ( I p B, I R B, q j ), where ( I p B, I R B ) denotes the origin and orientation of the ase frame expressed in the inertial frame, and q j denotes the joint angles. It is possile to define an operation associated with the set Q such that this set is a group. Given two elements q and ρ of the configuration space, the set Q is a group under the following operation: q ρ = (p q + p ρ, R q R ρ, q j + ρ j ). Furthermore, one easily shows that Q is a Lie group. Then, the velocity of the multi-ody system can e characterized y the algera V of Q defined y: V = R 3 R 3 R n. An element of V is then a triplet ν = ( I ṗ B, I ω B, q j ) = (v B, q j ), where I ω B is the angular velocity of the ase frame expressed w.r.t. the inertial frame, i.e. I Ṙ B = S( I ω B ) I R B. We also assume that the root is interacting with the environment exchanging n c distinct wrenches. The application of the Euler-Poincaré formalism 2, Ch. 3.5 to the multiody system yields the following equations of motion: n c M(q) ν + C(q, ν)ν + G(q) = Bτ + JC k f k () k= where M R n+6 n+6 is the mass matrix, C R n+6 n+6 is the Coriolis matrix, G R n+6 is the gravity term, B = ( n 6, n ) is a selector matrix, τ R n is a vector representing the actuation joint torques, and f k R 6 denotes the k-th external wrench applied y the environment on the root. We assume that the application point of the external wrench is associated with a frame C k, attached to the link on which the wrench acts, and has its z axis pointing in the direction of the normal of the contact plane. Then, the external wrench f k is expressed in a frame whose orientation is that of the inertial frame I, and whose origin is that of C k, i.e. the application point of the external wrench f k. The Jacoian J Ck = J Ck (q) is the map etween the root s velocity ν and the linear and angular velocity I v Ck := ( I ṗ Ck, I ω Ck ) of the frame C k, i.e. I v Ck = J Ck (q)ν. The Jacoian has the following structure: J Ck (q) = JC k (q) J j C k (q) R 6 n+6, (2a) JC k (q) = 3 S( I p Ck I p B ) R 6 6. (2) Lastly, it is assumed that holonomic constraints act on System (). These constraints are of the form c(q) =, and may represent, for instance, a frame having a constant pose w.r.t. the inertial frame. In the case where this frame corresponds to the location at which a contact occurs on a link, we represent the holonomic constraint as J Ck (q)ν=. C. Block-Diagonalization of the Mass Matrix This section recalls a new expression of the equations of motion (). In particular, the next lemma presents a change of coordinates in the state space (q, ν) that transforms the system dynamics () into a new form where the mass matrix is lock diagonal, thus decoupling joint and ase frame accelerations. The otained equations of motion are then used in the remaining of the paper. Lemma. The proof is given in 22. Consider the equations of motion given y () and the mass matrix partitioned as following M = M Mj M j M j As an ause of notation, we define as wrench a quantity that is not the dual of a twist

3 with M R 6 6, M j R 6 n and M j R n n. Perform the following change of state variales: with q := q, ν := T (q)ν, (3) c X T := c B X B M M j, (4a) n 6 n c X B := 3 S( I p c I p B ) (4) where the superscript c denotes the frame with the origin located at the center of mass, and with orientation of I. Then, the equations of motion with state variales (q, ν) can e written in the following form with M (q) = n c M(q) ν + C(q, ν)ν + G = Bτ + J C i f i, (5) M(q) = T MT = C(q, ν) = T (M T + CT ), i= M (q) 6 n, (6a) n 6 M j (q j ) (6) G = T G = mge 3, (6c) J Ci (q) = J Ci (q)t = J C i (q) J j C i (q j ), (6d) m3 3 3, J 3 3 I(q) C i (q)= 3 S(p Ci I p c ) where m is the mass of the root and I is the inertia matrix computed with respect to the center of mass, with the orientation of I. The aove lemma points out that the mass matrix of the transformed system (5) is lock diagonal, i.e. the transformed ase acceleration is independent from the joint acceleration. More precisely, ( the transformed ) root s velocity ν is given y ν = where Iṗ c is the velocity of the Iṗ c I ω c q j center-of-mass of the root, and I ω c is the so-called average angular velocity 2 24,25,26. Hence, Eq. (5) unifies what the specialized rootic literature usually presents with two sets of equations: the equations of motion of the free floating system and the centroidal dynamics 3 when expressed in terms of the average angular velocity. For the sake of correctness, let us remark that defining the average angular velocity as the angular velocity of the multi-ody system is not theoretically sound. In fact, the existence of a rotation matrix R(q) SO(3) such that Ṙ(q)R (q) = S( I ω c ), i.e. the integraility of I ω c, is still an open issue. Oserve also that the gravity term G is constant and influences the acceleration of the center-of-mass only. This 2 The term I ω c is also known as the locked angular velocity In the specialized literature, the terms centroidal dynamics are used to indicate the rate of change of the root s momentum expressed at the centerof-mass, which then equals the summation of all external wrenches acting on the multi-ody system 26. is a direct consequence of (3)-(4) and of the property that G(q) = Mge 3, with e 3 R n+6. Remark. In the sequel, we assume that the equations of motion are given y (5), i.e. the mass matrix is lock diagonal. As an ause of notation ut for the sake of simplicity, we hereafter drop the overline notation. D. A classical momentum-ased control strategy This section recalls a classical momentum-ased control strategy when implemented as a two-layer stack-of-task. We assume that the ojective is the control of the root momentum and the staility of the zero dynamics. Recall that the configuration space of the root evolves in a group of dimension 4 n + 6. Hence, esides pathological cases, when the system is suject to a set of holonomic constraints of dimension k, the configuration space shrinks into a space of dimension n + 6 k. The staility analysis of the constrained system may then require to determine the minimum set of coordinates that characterize the evolution of the constrained system. This operation is, in general, far from ovious ecause of the topology of the group Q. Now, in the case the holonomic constraint is of the form T (q) = constant, with T (q) R 3 SO(3), i.e. a root link has a constant position-and-orientation w.r.t. the inertial frame, one gets rid of the topology related prolems of Q y relating the ase frame B and the constrained frame. In this case, the minimum set of coordinates elongs to R n and can e chosen as the joint variales q j. In light of the aove, we make the following assumption. Assumption. Only one frame associated with a root link has a constant position-and-orientation with respect to the inertial frame. Without loss of generality, it is assumed that the only constrained frame is that etween the environment and one of the root s feet. Consequently, one has: n c k= J C k f k = J (q)f, (7) where J(q) R 6 n+6 is the Jacoian of a frame attached to the foot s sole in contact with the environment, and f R 6 the contact wrench. Differentiating the kinematic constraint associated with the contact, yield v J J B j + J q j J(q)ν = J J j ν = (8) J v B j =. (9) q j ) Momentum control: Thanks to the results presented in Lemma, the root s momentum H R 6 is given y H = M v B. The rate-of-change of the root momentum equals the net external wrench acting on the root, which in the present case reduces to the contact wrench f plus the gravity wrench. To control the root momentum, it is assumed that 4 With group dimension we here mean the dimension of the associated algera V.

4 the contact wrench f can e chosen at will. Note that given the particular form of (5), the first six rows correspond to the dynamics of the root s momentum, i.e. d dt (M v B ) = J f mge 3 = Ḣ(f) () where H := (H L, H ω ), with H L, H ω R 3 linear and angular momentum, respectively. The control ojective can then e defined as the stailization of a desired root momentum H d. Let us define the momentum error as follows H = H H d. The control input f in Eq. () is chosen so as Ḣ(f) = Ḣ := Ḣd K p H Ki I H I H = H (a) () where K p, K i R 6 6 are two symmetric, positive definite matrices. It is important to note that a classical choice for the matrix K i consists in,27: K i = ( Ki L ), (2) i.e. the integral correction term at the angular momentum level is equal to zero, while the positive definite matrix Ki L R 3 3 is used for tuning the tracking of a desired center-of-mass position when the initial conditions of the integral in () are properly set. Assumption implies that the contact wrench satisfying Eq. () can e chosen as f = J (Ḣ + mge 3 ). (3) Now, to determine the control torques that instantaneously realize the contact force given y (3), we use the dynamic equations (5) along with the constraints (9), which yield τ = Λ (JM (h J f) Jν) + N Λ τ (4) where Λ = J j M j R 6 n, N Λ R n n is the nullspace projector of Λ, h R n+6 is the vector containing oth the Coriolis and gravity terms and τ R n is a free variale. 2) Staility of the zero dynamics: The staility of the zero dynamics is usually attempted y means of a so called postural task, which exploits the free variale τ. A classical state-of-the-art choice for this postural task consists in: τ = h j Jj f Kj p(q j qj d) Kj d q j, where h j Jj f compensates for the nonlinear effect and the external wrenches acting on the joint space of the system. Hence, the (desired) input torques that are in charge of stailizing oth a desired root momentum H d and the associated zero dynamics are given y τ = Λ (JM (h J f) Jν) + N Λ τ f = J (Ḣ ) + mge 3 τ = h j J j f K j p(q j q d j ) K j d q j (5a) (5) (5c) We present elow numerical results showing that (5) with K i as (2) may lead to unstale zero dynamics. III. NUMERICAL EVIDENCE OF UNSTABLE ZERO DYNAMICS A. Simulation Environments Two different simulation setups have een exploited to perform the numerical validation. In oth cases, we simulate the humanoid root icu with 23 DoFs 28. ) Custom setup: It is in charge of integrating the dynamics (5) when it is suject to the constraint (9). We parametrize SO(3) y means of a quaternion representation Q R 4. The resulting state space system, which is integrated through time, is then: χ := (p B, Q, q j, ṗ B, ω B, q j ), and its derivative is given y χ = (ṗ B, Q, q j, ν) The constraints (9), as well as Q =, are then enforced during the integration phase, and additional correction terms have een added 29. The system evolution is then otained y integrating the constrained dynamical system with the numerical integrator MATLAB ode5s. 2) Gazeo setup: The Gazeo simulator 3 is the other simulation setup used for our tests. Of the different physic engines that can e used with Gazeo, we chose the Open Dynamics Engine (ODE). Differently from the previous simulation environment, Gazeo allows one for more flexiility. Indeed, we only have to specify the model of the root, and the constraints arise naturally while simulating. Furthermore, Gazeo integrates the dynamics with a fixed step semiimplicit Euler integration scheme. Another advantage of using Gazeo w.r.t. the custom integration scheme previously presented consists in the aility to test in simulation the same control software used on the real root. B. Unstale Zero Dynamics To show that the momentum-ased control strategies may lead to unstale zero dynamics, we control the linear momentum of the root so as to follow a desired center of mass trajectory, i.e. a sinusoidal reference along the y coordinate with amplitude.5m and frequency.3hz. The reference qj d is set equal to its joint initial value, i.e. qd j = q j(). ) Tests on the root alancing on one foot in the custom simulation setup: we present simulation results otained y applying the control laws (5) with K i as in (2). It is assumed that the left foot is attached to ground, and no other external wrench applies to the root. Figure shows typical simulation results of the convergence to zero of the root s momentum error, thus meaning that the wrench (3) ensures the stailization of H towards zero. Figure 2, instead, depicts the joint position error norm q j qj d. This figure shows that the norm of the joint angles increases while the root s momentum is kept equal to zero, which is a classical ehavior of unstale zero dynamics. 2) Tests when the root alances on two feet in the Gazeo simulation setup: Tests on the staility of the zero dynamics have een carried out also in the case the root stands on oth feet. The main difference etween the control algorithm running in this case and that in (5) resides in the choice of contact forces and in the holonomic constraints acting on the system. More precisely, the contact wrench f is now a twelve

5 jqj! q d j j rad jqj! q d j j rad j ~ Hj dimensional vector, and composed of the contact wrenches f L, f R R 6 etween the floor and left and right feet, respectively. Hence, f = f L, f R R 2. Also, let J L, J R R 6 n+6 denote the Jacoian of two frames associated with the contact locations of the left and right foot, respectively. Then, J = JL, J R R 2 n+6. By assuming that the contact wrenches can still e used as virtual control input in the dynamics of the root s momentum Ḣ in (), one is left with a six-dimensional redundancy of the contact wrenches to impose H(f) = Ḣ. We use this redundancy to minimize the joint torques. In the language of Optimization Theory, the aove control ojectives can e formulated as follows. f = argmin τ (f) (6a) f s.t. Cf < Ḣ(f) = Ḣ (6) (6c) τ (f) = argmin τ(f) τ (f) (7) τ s.t. J(q, ν)ν + J(q) ν = (8a) ν = M (Bτ + J (q)f h(q, ν)) (8) τ = h j J j f K j p(q j q d j ) K j d q j (8c) Note that the additional constraint (6) ensures that the desired contact wrenches f elong to the associated friction cones. Once the optimum f has een determined, the input torques τ are otained y evaluating the expression (7), i.e. τ = τ (f ) (9) Figure 3 depicts a typical ehavior of the joint position error norm q j qj d when the aove control algorithm is applied. It is clear from this figure that the instaility of the zero dynamics is oserved also in the case where the root stands on two feet. IV. CONTROL DESIGN To circumvent the prolems related to the staility of the zero dynamics discussed in the previous section, we propose a modification of the control laws (5) that allows us to show stale zero dynamics of the constrained, closed loop system. The following results exploit the so-called centroidalmomentum-matrix J G (q) R 6 n+6, namely the mapping etween the root velocity ν and the root s momentum H: H = J G (q)ν. Now, oserve that thanks to the results of Lemma one has J G (q) = M 6 n. Then, oserve that when Assumption is satisfied, Eq. (8) allows us to write the root s momentum linearly w.r.t. the root s joint velocity, i.e. H = J G (q j ) q j, where J G (q j ) R 6 n is: J G (q j ) := J L G (q j) J ω G (q j) = M J J j, (2) and J L G (q j), J ω G (q j) R 3 n. In light of the aove, the following result holds time s Fig.. Time evolution of the root s momentum error when standing on one foot and when the control law (5) is applied. Simulation run with the custom environment time s Fig. 2. Time evolution of the norm of the position error q j qj d when the root is standing on one foot and when the control law (5) is applied. Simulation run with the custom environment time s Fig. 3. Time evolution of the norm of the position error q j qj d when the root is standing on two feet and when the control law (6)- (9) is applied. Simulation run with the Gazeo environment. Lemma 2. Assume that Assumption holds, and that the root possesses more than six degrees of freedom, i.e. n 6. In addition, assume also that H d =. Let (q j, q j ) = (q d j, ) (2) denote the equilirium point associated with the constrained, closed loop system and assume that the matrix Λ = J j (q)m j (q) is full row rank in a neighorhood of (2). Apply the control laws (5) with I H = J L G (q j) J ω G (qd j ) q j (22) K i >, K j p = K j pn Λ M j, K j d = Kj d N ΛM j, (23) where K j p, K j d Rn n are two constant, positive definite matrices. Then, the equilirium point (2) of the constrained, closed loop dynamics is asymptotically stale. The proof is in Appendix. Lemma 2 shows that the asymptotic staility of the equilirium point (2) of the constrained, closed loop dynamics can e ensured y modifying the integral correction terms, and y modifying the gains of the postural task. As a consequence, the asymptotic staility of

6 the equilirium point (2) implies that the zero dynamics are locally asymptotically stale. The fact that the gain matrix K i must e positive definite conveys the necessity of closing the control loop with orientation terms at the angular momentum level. In fact, some authors intuitively close the angular momentum loop y using the orientation of the root s torso 7. The proof of Lemma 2 exploits the fact that the minimum coordinates of the root configuration space when Assumption holds is given y the joint angles q j. The analysis focuses on the closed loop dynamics of the form q j = f(q j, q j ) which is then linearized around the equilirium point (2). By means of a Lyapunov analysis, one shows that the equilirium point is asymptotically stale. One of the main technical difficulties when linearizing the equation q j = f(q j, q j ) comes from the fact that the closed loop dynamics depends on the integral of the root momentum, i.e. I H(t) = I H() + t J L G (q j(s)) J ω G (qd j ) q j (s) ds (24) The partial derivative of I H(t) w.r.t. the state (q j, q j ) is, in general, not ovious ecause the matrix JG L(q j) may not e integrale. Let us oserve, however, that the first three rows of I H(t) correspond to the velocity of the center-ofmass times the root s mass when expressed in terms of the minimal coordinates q j, i.e. JG L(q j) q j = mẋ c, with ẋ c R 3 the velocity of the root s center-of-mass. Clearly, this means that JG L(q j) is integrale, and that JG qj I H = L(q j) JG ω(qd j ) qj I H = (25) Remark 2. Lemma 2 suggests that applying the control laws (5) with the control gains as (23) can still guarantee staility and convergence of the equilirium point. First, oserve that the main difference etween the variale I H governed y the two expressions () and (22) resides only in the last three equations. Then, more importantly, note that the momentum H when Assumption holds can e expressed as follows H = J G (q d j ) q j +o(q j q d j, q j) which implies that t H ds = J G (q d j )(q j q d j ) + t o(q j q d j, q j ) ds As a consequence, the linear approximations of the integrals I H governed y () and (22) coincide when t lim o(q j qj d, q j) ds (q j qj d, qj) (q j qj d, q = (26) j) Under the aove assumption (26), the linear approximation of the control laws (5) when evaluated with () and (22) coincide, and staility and convergence of the equilirium point (qj d, ) can still e proven. V. SIMULATIONS AND EXPERIMENTAL RESULTS This section shows simulation and experimental results otained y applying the control laws (5)-(23) and (6)- (9)-(23). To show the improvements of the control modification in Lemma 2, we apply the same reference signal of Section III, which revealed unstale zero dynamics. Hence, the desired linear momentum is chosen so as to follow a sinusoidal reference on the center of mass. Also, control gains are kept equal to those used for the simulations presented in Section III. A. Simulation results Figures 4 5 show the norm of the joint errors q j q d j when the root stands on either one or two feet, respectively. Experiments on two feet have een performed to verify the roustness of the new control architecture. The simulations are performed oth with the custom and Gazeo environment. As expected, the zero dynamics is now stale, and no divergent ehavior of the root joints is oserved. B. Results on the icu Humanoid Root We then went one step further and implemented the control algorithm (6)-(9) with the modification presented in Lemma 2 on the real humanoid root. The rootic platform used for testing is the icu humanoid root 28. For the purpose of the proposed control law, icu is endowed with 23 degrees of freedom. A low level torque control loop, running at khz, is responsile for stailizing any desired torque reference signal. Figures 6 7 show the joint position error q j q d j and the center of mass error. Though the center of mass does not converge to the desired value, all signals are ounded, and the control modification presented in Lemma 2 does not pose any arrier for the implementation of the control algorithm (6)- (9) on a real platform. VI. CONCLUSIONS AND FUTURE WORKS Momentum-ased controllers are an efficient control strategy for performing alancing and walking tasks on humanoids. In this paper, we presented numerical evidence that a stack-of-task approach to this kind of controllers may lead to an instaility of the zero dynamics. In particular, to ensure staility, it is necessary to close the loop with orientation terms at the momentum level. We show a modification of state-of-the-art momentum ased control strategies that ensure asymptotic staility, which was shown y performing Lyapunov analysis on the linearization of the closed loop system around the equilirium point. Simulation and experimental tests validate the presented analysis. The stack-of-task approach strongly resemles a cascade of dynamical systems. It is the authors opinion that the staility of the whole system can e proved y using the general framework of staility of interconnected systems. A critical point needed to prove the staility of constrained dynamical system is to define the minimum set of coordinates identifying its evolution. This can e straightforward in case of one contact, ut the extension to multiple contacts is not trivial and must e considered carefully.

7 jqj! q d j j rad ~xg cm jqj! q d j j rad jqj! q d j j rad time s Fig. 4. Time evolution of the norm of the position error q j qj d when the root is standing on one foot and when the control law (5) (23) (22) is applied. Simulation run with the custom environment time s Fig. 5. Time evolution of the norm of the position error q j qj d when the root is standing on two feet and when the control law (6) (9) (23) (22) is applied. Simulation run with the Gazeo environment. APPENDIX: PROOF OF LEMMA 2 As descried in Section IV the proof is composed of two steps. First, we linearize the constrained closed loop dynamics around the equilirium (qj d, ). Then, y means of Lyapunov analysis, we show that the equilirium point is asymptotically stale. ) Linearization: Consider that Assumption holds, and that we apply the control laws (5)-(22) with the gains as (23). The closed loop joint space dynamics of system (5) constrained y (9) is given y the following equation: M j q j + h j J j f = τ (27) Now, rewrite (5) as follows: τ = Λ + (Λ(h j Jj f) + J M (h J f) Jν) + N Λ τ. Therefore, Eq. (27) can e simplified into: q j = M j (Λ + (J M (h J f) Jν) N Λ u ) (28) where u = Kp(q j j qj d) + Kj d q j, while h = C ν + C j q j + mge 3, and C R 6 6, C j R 6 n, C j R n 6, C j R n n are otained from the following partition of the Coriolis matrix: C C C = j C j C j Sustituting (3) into (28) and grouping together the terms that are linear with respect of joint velocity yield: q j = M j Λ (J M Ḣ + Γ q j ) + N Λ u (29) where Γ = J j J M C j + (J M C J )J J j. Define the state x as x := x x 2 = time s Fig. 6. Time evolution of the position error norm q j qj d when the root is standing on two feet and when the laws (6) (9) (23) (22) are applied. Experiment run on the humanoid root icu ~xgx ~xgy ~xgz time s Fig. 7. Time evolution of root center-of-mass errors x G when the root stands on two feet and when the laws (6) (9) (23) (22) are applied. Experiment run on the humanoid root icu. qj qd j q j. Since H d, the linearized dynamical system aout the equilirium point (qj d, ) is given y qj ẋ ẋ = qj ẋ n n x = n x (3) qj ẋ 2 qj ẋ 2 A A 2 To find the matrices A, A 2 R n n, one has to evaluate the following partial derivatives 6 y q j = y (M j Λ J M e i )e i Ḣ M j N Λ y u i= n i= y (M j N Λ e i )e i u M Λ J M y Ḣ with y = {q j, q j }. Note that Ḣ = and u = when evaluated at q j = qj d and q j =. We thus have to compute only the partial derivatives of Ḣ and u. The latter is trivially given y qj u = K j pn Λ M j and qj u = K j d N ΛM j. The former can e calculated via Eq. (25). In light of the aove we otain the expressions of the matrices in (3): A = M j Λ J M K i M J J j M j N Λ K j pn Λ M j A 2 = M j Λ J M K p M J J j M j N Λ K j d N ΛM j. 2) Proof of Asymptotic Staility: Consider now the following Lyapunov candidate: V (x)= x Mj Q M j x +x 2 Mj Q 2 M j x 2 2 where M j = M j (qj d ), and Q := Λ J M K i M J Λ + N Λ K j pn Λ Q 2 := Λ J M M J Λ + N Λ j

8 calculated at x =, x 2 =. V is a properly defined candidate, in fact V = x = and is positive definite otherwise. Indeed, Q can e rewritten in the following way: Q = Λ N Λ J M K im J Λ K j p N Λ. and, ecause Λ and N Λ are orthogonal Q is positive definite. The same reasoning can e applied to Q 2. We can now consider the time derivative of V : V = x Mj Q M j x 2 + x 2 Mj Q 2 M j ẋ 2 = x 2 Mj (Λ J M K p M J Λ +N Λ K j d N Λ)M j x 2. The staility of the equilirium point x = associated with the linear system (3) thus follows. To prove the asymptotic staility of the equilirium point x =, which implies its asymptotic staility when associated with the nonlinear system (29), we have to resort to LaSalle s invariance principle. Let us define the invariant set S := {x : V (x) = } that implies S = {(x, )}. It is easy to verify that the only trajectory starting in S and remains in S is given y x = thus proving LaSalle s principle. As a consequence, the equilirium point x = (q j, q j ) = (qj d, ) is asymptotically stale. REFERENCES S. Caux, E. Mateo, and R. Zapata, Balance of iped roots: special doule-inverted pendulum, Systems, Man, and Cyernetics, IEEE International Conference on, K. Hirai, M. Hirose, Y. Haikawa, and T. Takenaka, The development of Honda humanoid root, Rootics and Automation, 998. Proceedings. 998 IEEE International Conference on, S. Hyon, J. Hale, and G. Cheng, Full-ody compliant humanhumanoid interaction: Balancing in the presence of unknown external forces, Rootics, IEEE Transactions on, Oct Q. Huang, K. Kaneko, K. Yokoi, S. Kajita, T. Kotoku, N. Koyachi, H. Arai, N. Imamura, K. Komoriya, and K. Tanie, Balance control of a iped root comining off-line pattern with real-time modification, Rootics and Automation, 2. Proceedings. ICRA. IEEE International Conference on, 2. 5 R. Featherstone, Rigid Body Dynamics Algorithms. Secaucus, NJ, USA: Springer-Verlag New York, Inc., J. A. Acosta and M. Lopez-Martinez, Constructive feedack linearization of underactuated mechanical systems with 2-DOF, Decision and Control, 25 and 25 European Control Conference. CDC-ECC 5. 44th IEEE Conference on, C. Ott, M. Roa, and G. Hirzinger, Posture and alance control for iped roots ased on contact force optimization, in Humanoid Roots (Humanoids), 2 th IEEE-RAS International Conference on, Oct 2, pp P. Wensing and D. Orin, Generation of dynamic humanoid ehaviors through task-space control with conic optimization, in Rootics and Automation (ICRA), 23 IEEE International Conference on, May 23, pp F. Nori, S. Traversaro, J. Eljaik, F. Romano, A. Del Prete, and D. Pucci, icu whole-ody control through force regulation on rigid noncoplanar contacts, Frontiers in Rootics and AI, vol. 2, no. 6, 25. B. Stephens and C. Atkeson, Dynamic alance force control for compliant humanoid roots, in Intelligent Roots and Systems (IROS), 2 IEEE/RSJ International Conference on, Oct 2, pp A. Herzog, L. Righetti, F. Grimminger, P. Pastor, and S. Schaal, Balancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics, in Intelligent Roots and Systems (IROS 24), 24 IEEE/RSJ International Conference on, Sept 24, pp T. Koolen, S. Bertrand, G. Thomas, T. de Boer, T. Wu, J. Smith, J. Englserger, and J. Pratt, Design of a momentum-ased control framework and application to the humanoid root atlas, International Journal of Humanoid Rootics, vol. 3, p. 34, March L. Righetti, J. Buchli, M. Mistry, and S. Schaal, Inverse dynamics control of floating-ase roots with external constraints: A unified view, IEEE International Conference on Rootics and Automation, May 2. 4, Control of legged roots with optimal distriution of contact forces, in Humanoid Roots (Humanoids), 2 th IEEE-RAS International Conference on, Oct 2, pp J. Nakanish, M. Mistry, and S. Schaal, Inverse Dynamics Control with Floating Base and Constraints, Rootics and Automation, 27 IEEE International Conference on, pp , M. Hopkins, R. Griffin, A. Leonessa, B. Lattimer, and T. Furukawa, Design of a compliant ipedal walking controller for the darpa rootics challenge, in Humanoid Roots (Humanoids), 25 IEEE- RAS 5th International Conference on, Nov 25, pp P.-B. Wieer, Holonomy and Nonholonomy in the Dynamics of Articulated Motion. Berlin, Heidelerg: Springer Berlin Heidelerg, 26, pp Online. Availale: M. Liu and V. Padois, Reactive whole-ody control for humanoid alancing on non-rigid unilateral contacts, Intelligent Roots and Systems (IROS), 25 IEEE/RSJ International Conference on, pp , A. Hofmann, M. Popovic, and H. Herr, Exploiting angular momentum to enhance ipedal center-of-mass control, Rootics and Automation. ICRA 9. IEEE International Conference on. 29, H. Herr and M. Popovic, Angular momentum in human walking, Journal of Experimental Biology, vol. 2, no. 4, pp , J. E. Marsden and T. S. Ratiu, Introduction to Mechanics and Symmetry: A Basic Exposition of Classical Mechanical Systems. Springer Pulishing Company, Incorporated, S. Traversaro, D. Pucci, and F. Nori, On the ase frame choice in free-floating mechanical systems and its connection to centroidal dynamics, Sumitted to Humanoid Roots (Humanoids), 26 IEEE-RAS International Conference on, 26. Online. Availale: 23 J. E. Marsden and J. Scheurle, The reduced euler-lagrange equations, Fields Institute Comm, vol., pp , J. Jellinek and D. Li, Separation of the energy of overall rotation in any n-ody system, Physical review letters, vol. 62, no. 3, p. 24, H. Essén, Average angular velocity, European journal of physics, vol. 4, no. 5, p. 2, D. Orin, A. Goswami, and S.-H. Lee, Centroidal dynamics of a humanoid root, Autonomous Roots, S.-H. Lee and A. Goswami, Ground reaction force control at each foot: A momentum-ased humanoid alance controller for non-level and non-stationary ground, in Intelligent Roots and Systems (IROS), 2 IEEE/RSJ International Conference on, Oct 2, pp G. Metta, L. Natale, F. Nori, G. Sandini, D. Vernon, L. Fadiga, C. von Hofsten, K. Rosander, M. Lopes, J. Santos-Victor, A. Bernardino, and L. Montesano, The icu humanoid root: An open-systems platform for research in cognitive development, Neural Networks, vol. 23, no. 89, pp , 2, social Cognition: From Baies to Roots. 29 S. Gros, M. Zanon, and M. Diehl, Baumgarte stailisation over the SO(3) rotation group for control, 25 54th IEEE Conference on Decision and Control (CDC), pp , Decemer N. Koenig and A. Howard, Design and use paradigms for gazeo, an open-source multi-root simulator, Intelligent Roots and Systems, 24. (IROS 24). Proceedings. 24 IEEE/RSJ International Conference on, pp , 24.