Automatic Gain Tuning of a Momentum Based Balancing Controller for Humanoid Robots  Daniele Pucci, Gabriele Nava and Francesco Nori 1  Abstract — This paper proposes a technique for automatic gain tuning of a momentum based balancing controller for humanoid robots. The controller ensures the stabilization of the   centroidal   dynamics   and   the   associated   zero   dynamics. Then,   the   closed-loop,   constrained   joint   space   dynamics   is linearized and the controller’s gains are chosen so as to obtain desired   properties   of   the   linearized   system.   Symmetry   and positive definiteness constraints of gain matrices are enforced by proposing a tracker for symmetric positive definite matrices. Simulation results are carried out on the humanoid robot iCub.  I. I NTRODUCTION  Humanoid robotics is an undoubtedly flourishing field of research. Locomotion and manipulation have received a special attention from the control community, and the results shown at the DARPA robotics challenge are both stimulating and promising [1], [2]. Despite these advances, robust controllers for balancing and walking of humanoids still   require   a   special   focus   of   the   robotics   community. Furthermore, when these controllers are implemented on real platforms, the achievement of desired system performances usually requires time consuming tuning of the (often very numerous) gains characterizing the control laws. This paper proposes a technique to tune automatically the gains of a momentum based balancing strategy for humanoid robots. A classical approach to the modelling and control of humanoid robots is based on considering the robot attached to ground, i.e. the robot is considered to be   fixed-base   [3]. In this case, in fact, well-known classical control strategies for manipulators can be directly applied for robot control. The limitations of this approach arise when attempting to tackle the general control problem of a humanoid interacting with its surrounding environment. For instance, running involves flight phases where the fixed-base approach clearly fails. At the modelling level, the Euler-Poincar` e equations pro- vide singularity free equations of motion for the humanoid robot [4, Chapter 13], and efficient algorithms can be applied for   determining   the   components   of   these   equations   [5]. When considering these equations of motion, the mechanical system representing the humanoid robot is usually under actuated, and this forbids the full feedback linearization of the closed-loop system [6], [5]. The system underactuation is usually dealt with by means of constraints that arise from the contacts between the robot and the environment. This requires a close attention to the forces the robot exerts on  *This paper was supported by the FP7 EU project KoroiBot (No. 611909 ICT 2013.10 Cognitive Systems and Robotics)  1   The authors are with the iCub Facility department, Istituto Italiano di Tecnologia, Via Morego 30, Genoa, Italy   name.surname@iit.it  the environment since uncontrolled forces may break the contacts, thus rendering the control of the robot critical. To ensure that the contact forces respect some physical constraints, different optimization procedures can be applied [7], [8].  Task-based   control strategies have proven to be an efficient solution for balancing and walking of humanoid robots [9], [10], [11]. The aim of these strategies is the achievement of several control objectives, which are organized in a hierar- chical structure. The possibility of defining different control objectives with different priorities is an efficient way to deal with manipulation tasks while balancing [12]. Furthermore, high priority tasks may be used to control directly the contact forces the robot exerts at the contact points, e.g. through the control of the center of mass dynamics [13]. When the above control algorithms are implemented in real applications, a long and tedious tuning of control gains is often required to achieve desired system performances. Despite the large number of gain optimization procedures for classical dynamical systems (see, e.g., [14], [15]) gain optimization techniques for floating base systems, and in particular in the field of humanoid robots, still needs more investigations. Preliminary results in this direction consist in   applying   classical   LQR   approaches   to   the   linearized humanoid robot dynamics [16], [17]. In particular, LQR based optimization techniques can be applied to the so-called centroidal dynamics [16]. Another approach may consist of considering simplified robot models, and then apply classical gain scheduling procedures for balancing purposes [18]. In this paper, we propose a gain tuning method for the momentum-based control strategy we implemented on the iCub humanoid robot [19]. Contrary to [16], we optimize the gains by imposing desired properties of the linearized joint space dynamics. The approach handles any number of contacts between the robot and the environment. Symmetry and positive definiteness constraints of gain matrices are en- forced via a nonlinear tracker for symmetric positive definite matrices. Simulation results verify the presented approach. The paper is organized as follows. Section II introduces notation and system modelling. Section III recalls and com- plements the momentum-based control strategy [19]. Sec- tion IV presents the linearization and the gain optimization procedure. Section V presents simulations results using the iCub robot model. Conclusions and future works conclude the paper.  arXiv:1610.02849v3 [cs.SY] 21 Oct 2016
II. BACKGROUND  A. Notation  •   I   defines an inertial frame of reference, with its   z   axis pointing against the gravity. The constant   g   denotes the norm of the gravitational acceleration.  •   Given a matrix   A   ∈   R m × n , we denote with   A †   ∈   R n × m  its Moore Penrose pseudoinverse.  •   e i   ∈   R m   is the canonical vector, consisting of all zeros but the   i -th component that is equal to one.  •   We denote with   m   the total mass of the robot.  B. Modelling  The robot is modelled as a multi-body system composed of   n   + 1   rigid bodies, called links, connected by   n   joints with one degree of freedom each. We also assume that none of the links has an   a priori   constant pose with respect to an inertial frame, i.e. the system is   free floating . The robot configuration space is the Lie group   Q   =  R 3   ×   SO (3)   ×   R n   and it is characterized by the   pose  (position and orientation) of a base frame attached to a robot’s link, and the joint positions. An element   q   ∈   Q  can be defined as the following triplet:   q   = ( I   p B ,   I   R B , q j   )  where   I   p B   ∈   R 3   denotes the position of the base frame with respect to the inertial frame,   I   R B   ∈   R 3 × 3   is a rotation matrix representing the orientation of a   base frame , and   q j   ∈   R n  is the joint configuration. The velocity of the multi-body system can be characterized by the   algebra   of the group defined as   V   =   R 3   ×   R 3   ×   R n . An element of   V   is a triplet  ν   = ( I    ̇ p B , I   ω B ,    ̇ q j   ) = ( v B ,    ̇ q j   ) , where   I   ω B   is the angular velocity of the base frame expressed w.r.t. the inertial frame, i.e.   I    ̇ R B   =   S ( I   ω B ) I   R B . A more detailed description of the floating base model is provided in [19]. We assume that the robot interacts with the environment by exchanging   n c   distinct wrenches. The equations of motion of the multi-body system can be described applying the Euler- Poincar ́ e formalism [20, Ch. 13.5]:  M   ( q )  ̇ ν   +   C ( q, ν ) ν   +   G ( q ) =   Bτ   +  n c ∑  k =1  J > C k   f k   (1) 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   = (0 n × 6 ,   1 n ) >   is a selector matrix,   τ   ∈   R n   is a vector representing the internal actuation torques, and   f k   ∈   R 6  denotes an external wrench applied by the environment to the link of the   k -th contact. The Jacobian   J C k   =   J C k   ( q )   is the map between the robot’s velocity   ν   and the linear and angular velocity at the   k -th contact link. As described in [21, Sec. 5], it is possible to apply a coordinate transformation in the state space   ( q, ν )   that transforms the system dynamics (1) into a new form where the mass matrix is block diagonal, thus decoupling joint and base frame accelerations. Also, in this new set of coordinates, the first six rows of Eq. (1) are the   centroidal dynamics 1 . As  1 In the specialized literature, the term   centroidal dynamics   is used to indicate the rate of change of the robot’s momentum expressed at the center- of-mass, which then equals the summation of all external wrenches acting on the multi-body system [22].  an abuse of notation, we assume that system (1) has been transformed in this new set of coordinates, i.e.  M   ( q ) =  [ M b ( q )   0 6 × n  0 n × 6   M j   ( q )  ]  ,   H   =   M b v B ,   (2) where   M b   ∈   R 6 × 6 , M j   ∈   R n × n ,   H   := ( H L , H ω   )   ∈   R 6   is the robot momentum, and   H L , H ω   ∈   R 3   are the linear and angular momentum at the center of mass, respectively. Lastly, it is assumed that a set of holonomic constraints acts on System (1). These holonomic constraints are of the form   c ( q ) = 0 , 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 rigid contact occurs on a link, we represent the holonomic constraint as   J C k   ( q ) ν   = 0 .   Hence, the kinematic constraint associated with all the rigid contacts can be represented as  J ( q ) ν =      J C 1   ( q )  · · ·  J C nc   ( q )      ν =   [ J b   J j  ]   ν   =   J b v B   +   J j    ̇ q j   = 0 ,   (3) with   J b   ∈   R 6 n c   × 6 , J j   ∈   R 6 n c   × n , and    ̇ q j   ∈   R n   the joint space velocity. The base frame velocity is denoted by v B   ∈   R 6 , which in the new coordinates yielding a block-diagonal mass matrix is given by v B   = (  ̇ p c , ω o ) , where    ̇ p c   ∈   R 3   is the velocity of the system’s center of mass   p c   ∈   R 3 , and   ω o   ∈  R 3   is the so-called system’s   average angular velocity . By differentiating the kinematic constraint Eq. (3), one obtains  J    ̇ ν   +  ̇ Jν   =   J b    ̇ v B   +   J j    ̈ q j   +  ̇ J b v B   +  ̇ J j    ̇ q j   = 0 .   (4) III. RECALLS AND COMPLEMENTS ON THE MOMENTUM-BASED CONTROL STRATEGY We recall and complement the   momentum-based   control strategy implemented on our iCub humanoid robot [19]. The control objective is the stabilization of a desired robot momentum and the stability of the associated zero dynamics.  A. Momentum Control  Recall that the rate-of-change of the robot momentum equals the net external wrench acting on the robot, which in the present case reduces to the contact wrench   f   := ( f 1 ,   · · ·   , f n c   )   ∈   R 6 n c   plus the gravity wrenches. Then, in view of Eq. (2), the rate-of-change of the robot momentum can be expressed as:  d d t   ( M b v B ) =    ̇ H ( f   ) =   J >  b   f   −   mge 3 ,   (5) where   e 3   ∈   R 6 . The control objective is defined as the stabilization of a desired robot momentum   H d   ∈   R 6 . Let    ̃ H   =   H   − H d   define the momentum error. Assuming that the contact wrenches   f  can be chosen at will, then we choose   f   such that [19]:   ̇ H ( f   ) =    ̇ H ∗   :=    ̇ H d   −   K p    ̃ H   −   K i I    ̃ H   (6a)   ̇ I    ̃ H   =  [  J L G   ( q j   )  J ω G ( q d j   )  ]   ̇ q j   (6b)
K p , K i ∈ R 6 × 6   two symmetric, positive definite matrices and   ̄ J G ( q j   ):=   −   M b J †  b   J j   =  [  J L G   ( q j   )  J ω G ( q j   )  ]  ∈   R 6 × n , J L G   , J ω G   ∈   R 3 × n  If   n c   >   1 , there are infinite contact wrenches   f   that satisfy Eq. (6a). We parametrize the set of solutions   f   to (6a) as:  f   =   f 1   +   N b f 0   (7) with   f 1   =   J >†  b  (    ̇ H ∗   +   mge 3  )  ,   N b   ∈   R 6 n c   × 6 n c   the projector into the null space of   J >  b   , and   f 0   ∈   R 6 n c   the wrench redun- dancy that does not influence    ̇ H ( f   ) =    ̇ H ∗ . To determine the control torques that instantaneously realize the contact wrenches given by (7), we use the dynamic equations (1) along with the constraints (4), which yields:  τ   = Λ † ( JM   − 1 ( h   −   J > f   )   −    ̇ Jν ) +   N Λ τ 0   (8) with   Λ =   J j   M j   − 1   ∈   R 6 n c   × n ,   N Λ   ∈   R n × n   the projector onto the nullspace of   Λ , the vector   h   :=   C ( q, ν ) ν   +   G ( q )   ∈  R n +6 , and   τ 0   ∈   R n   a free variable.  B. Stability of the Zero Dynamics  The stability of the zero dynamics is attempted by means of a so called   postural task , which exploits the free variable  τ 0 . Partition the vector   h   as follows:   h   = ( h b , h j   ) , h b   ∈  R 6 , h j   ∈   R n . A choice of the postural task that ensures the stability of the zero dynamics on one foot is [19]:  τ 0   =   h j   −   J >  j   f   +   u 0   (9) where   u 0   :=   − K j p   N Λ M j   ( q j   −   q d j   )   −   K j d   N Λ M j    ̇ q j   , and  K j p   ∈   R n × n   and   K j d   ∈   R n × n   two symmetric, positive definite matrices. An interesting property of the closed loop system (1)–(8)–(9)–(7) is stated in the following Lemma.  Lemma 1.   The closed loop joint space dynamics    ̈ q j   does not depend upon the wrench redundancy   f 0 .  The proof of Lemma 1 is in the appendix. This result implies also that the linearization of the closed-loop joint dynamics does not depend on   f 0 , thus rendering the gain tuning procedure presented in this paper independent from the choice of the contact wrenches redundancy. This redun- dancy is exploited to minimize the joint torques   τ   in Eq. (8). In the language of   Optimization Theory , we can rewrite the control strategy as follows:  f   ∗  0   = argmin  f 0  | τ   ∗ ( f 0 ) | 2   (10a)  s.t. Cf 0   < b   (10b)  τ   ∗ ( f 0 ) = argmin  τ  | τ   ( f 0 )   −   τ 0 ( f 0 ) | 2   (10c)  s.t.   ̇ J ( q, ν ) ν   +   J ( q )  ̇ ν   = 0   (10d)   ̇ ν   =   M   − 1 ( Bτ   +   J > ( f 1   +   N b f 0 ) − h )   (10e)  τ 0   =   h j   −   J >  j   ( f 1   +   N b f 0 ) +   u 0 .   (10f) The   constraints   (10b)   ensure   the   satisfaction   of   friction cones, normal contact surface forces, and center-of-pressure constraints. The control torques are then given by   τ   = τ   ∗ ( f   ∗  0   ) . IV. GAIN TUNING PROCEDURE  A. Problem Statement  The goal is to impose desired local properties of the joint dynamics. The choice of focusing on the joint dynamics over other output functions reflects the aim of choosing stiffness and damping at the joint level, without perturbing the task hierarchy of momentum control and stability of the associated zero dynamics via postural control. Assuming that (10b) is always satisfied, the control torques obtained by solving the optimization problem (10) depend only on the system state, i.e.   τ   =   τ   ( q, ν ) . Since it is assumed that the robot stands on (at least) one foot, one can express the system state in terms of the joint position and velocity, i.e.   ( q, ν ) = ( q ( q j   ) , ν ( q j   ,    ̇ q j   )) . Then, the joint space dynamics depends only on joint position and velocity, i.e.    ̈ q j   =   f   ( q j   ,    ̇ q j   )   and we can linearize this dynamics about an equilibrium point   ( q d j   ,   0) . The process of finding the linearized joint dynamics is similar to that presented in [19], which yields   ̈ q j   =   − Q 1 ( q j   −   q d j   )   −   Q 2    ̇ q j  where   Q 1 , Q 2   ∈   R n × n   are given by  Q 1   =   C 1 ( q d j   ) K i C 2 ( q d j   ) +   C 3 ( q d j   ) K j p   C 4 ( q d j   )   (11a)  Q 2   =   C 1 ( q d j   ) K p C 2 ( q d j   ) +   C 3 ( q d j   ) K j d   C 4 ( q d j   )   (11b) and   C 1   =   M   − 1  j   Λ † J b M   − 1  b   ,   C 2   =   M b J †  b   J j   ,   C 3   =   M   − 1  j   N Λ  and   C 4   =   N Λ M j   . Now, let   x   ∈   R 2 n   be defined as follows  x   :=  [  x >  1   x >  2  ] >  =  [  q >  j   −   q d >  j    ̇ q >  j  ] >  .   The linearized joint space dynamics around an equilibrium point ( q d j   ,   0 ) is given by   ̇ x   =  [ ∂ q j    ̇ x 1   ∂    ̇ q j    ̇ x 1  ∂ q j    ̇ x 2   ∂    ̇ q j    ̇ x 2  ]  x   =  [   0 n   1 n  − Q 1   − Q 2  ]  x   =   Ax.   (12) Then, the optimization problem we attempt at solving is stated next.  K j ∗  p   , K j ∗  d   , K ∗  i   , K ∗  p   = argmin  K j p   ,K j d   ,K i ,K p  | A ( K j p   , K j d   , K i , K p ) − A d | 2   (13)  s.t. K i   =   K >  i   >   0   (14a)  K p   =   K >  p   >   0   (14b)  K j p   =   K j >  p   >   0   (14c)  K j d   =   K j >  d   >   0   (14d) where   A d   is the desired state matrix of the following form:  A d   =  [  0 n   1 n  − Q d  1   − Q d  2  ]  with   Q d  1 , Q d  2   ∈   R n × n   the desired stiff- ness and damping matrices. The optimization problem (13)- (14) may be solved with any nonlinear available optimizer. Yet, finding numerical solutions to the optimization problem may be time consuming, which may forbid the on-line use of the optimizer when the desired stiffness and damping are time-varying. In this case, the solutions to (13)-(14) may also become discontinuous at some time instants. We propose below a method for solving on-line the problem (13) that provide continuous solutions for the control gains.
B. Solution to the unconstrained problem  Assume that the constraints (14) do not hold. When the robot stands on one foot, intuition would suggest that the joint space dynamics can be imposed at will, i.e. there always exist control gains such that the matrices   Q d  1 , Q d  2   can be chosen arbitrarily. This section shows, however, that this is not possible because of the two strict stack-of-task control strategy defined in section III. To show this, we prove that there exist some matrices  Q d  1   such that no choice of the control gains renders   Q d  1   =  Q 1 ( K i , K j p   )   satisfied. Now, if the constraints (14) do not hold, then the opti- mization problem (13) can be rewritten as  y 1   = Γ  [  k i  k j p  ]  (15a)  Γ =  [[  C >  2   ⊗   C 1  ]   [  C >  4   ⊗   C 3  ]]  ∈   R n 2   × (36+ n 2 ) (15b) where   y 1 , k j p   ∈   R n 2  ,   k i   ∈   R 36   are the vectorization of ma- trices   Q 1 , K j p   and   K i   obtained by reordering their columns into a single column vector, and   ⊗   the Kronecker product. Then, the following result holds.  Lemma 2.   Assume that rank ( J ) = 6 n c , and that   n >   6 n c . Then, the matrix   Γ   is not full row rank, i.e. rank (Γ)   < n 2 .  The proof of the above Lemma is in the Appendix. As a consequence of the above Lemma, there exist some desired matrices   Q d  1 , i.e.   y 1 , such that no control gain implies the exact solution to   Q d  1   =   Q 1 ( K i , K j p   ) . On the other hand, the least square solution to the problem (15a) is given by  [  k ∗  i  k j ∗  p  ]  = Γ † y 1   (16) Clearly, the gains   K ∗  i   , K j ∗  p   obtained by the above solution do not in general satisfy the symmetry and positive definite- ness constraints (14). A similar procedure can be applied to find the gains   K j ∗  d   , K ∗  p   that do not in general satisfy (14) but solve in the least square sense   Q d  2   =   Q 2 ( K j ∗  d   , K ∗  p   ) .  C. Enforcing symmetry and positive definiteness constraints  In the previous section, we solved the problem (13) by assuming   that   the   constraints   (14)   do   not   hold.   Hence, we are now given with a set of gains   K j ∗  p   , K j ∗  d   , K ∗  i   , K ∗  p  that may not be symmetric and positive definite. Define  K ∗   :=   { K j ∗  p   , K j ∗  d   , K ∗  i   , K ∗  p   }   a matrix of proper dimension. Then, to enforce symmetry and positive definiteness con- straints, we solve (on-line) a second optimization problem for each of the unconstrained optimal gain. More precisely, the problem we solve follows:  O ∗ , L ∗   =   argmin  ( O,L )  | K ∗   −   X ( O, L ) | 2   (17) s.t.  OO >   = 1  L   diagonal matrix. with   X   :=   O >   exp( L ) O ,   O   an orthogonal matrix, and   L   a diagonal matrix. The solution to the problem (17) are the matrices   O ∗ , L ∗ . Therefore, the constrained optimized gain matrix is given by:  X ∗   =   O ∗>   exp( L ∗ ) O ∗   (18) Clearly, at this point we just moved the problem from solving the optimization problem (13) with the constraints (14) to solving the problems (17) with the constraints of the kind  OO >   = 1 . Now, being   O   an orthogonal matrix, then    ̇ O   =  OS ( v ) , with   v   a vector of proper dimension depending on the dimension of   O , and   S ( · )   a skew symmetric matrix. Assuming   v   and    ̇ L   as exogenous control inputs, one can find Lyapunov-like solutions to the problem (17). More precisely, the following result holds.  Lemma 3.   Let   O, L   ∈   R m × m   denote an orthogonal and a diagonal matrix, respectively. Consider the following system:   ̇ L   =   U   (19a)   ̇ O   =   OS ( v )   (19b)  where the vector   v   ∈   R m ( m − 1) / 2   and the diagonal ma- trix   U   are considered as exogenous control inputs. Define   ̃ K   =   K ∗   −   X ( O, L ) , and the operator   S − 1 ( v )   such that  v   =   S − 1 ( S ( v )) . Apply the control inputs  U   =   −   K U   exp( − L ) diag ( B 1 )   (20a)  v   =   K v   S − 1  (   B 2 − B >  2  2  )  (20b)  to   system   (19) ,   where   K U   is   a   positive   definite   diag- onal   matrix,   K v   is   a   symmetric   positive   definite   ma- trix,   B 2   =   O >   exp( L ) OK ∗>   −   K ∗> O >   exp( L ) O ,  B 1   =   exp( L )   −   OK ∗ O > , and diag ( B 1 )   defined as follows diag ( B 1 ) ( i,j )   =   B 1( i,j )   if   i   =   j  diag ( B 1 ) ( i,j )   =   0   if   i   6   =   j.  Then, the following results hold:  •   If   K ∗   is symmetric and positive definite, the equilibrium point of the closed-loop dynamics    ̃ K   = 0   is stable;  •   The system trajectories    ̃ K ( t )   are globally bounded for any   K ∗   ∈   R m × m ;  •   |    ̃ K ( t ) | ≤ |    ̃ K (0) |   for any   K ∗   ∈   R m × m .  The proof of Lemma 3 is in the Appendix. The above Lemma points out that the   distance   between the optimal, uncon- strained solution   K ∗   (obtained in Section IV-B) and the con- strained (symmetric, positive definite) gain   X ( O ( t ) , L ( t ))  is non increasing, i.e.   |    ̃ K ( t ) | ≤ |    ̃ K (0) | . Then, the control laws (20) can be viewed as a tracker for symmetric positive definite matrices even when the matrix has to track a non symmetric positive definite matrix (i.e. it does not belong to the same manifold). Let us remark that convergence of the tracking error    ̃ K   to zero is not ensured   a priori . Simulations we have performed, however, tend to show that the cases when    ̃ K   does not converge to zero are limited, and the analysis on this convergence is currently being developed.
Note also that if the optimal, unconstrained solution   K ∗  varies in time slowly, the tracker preserves its properties by continuity. Then, one may think of applying the so- lution (16) (20) on-line for time-varying desired stiffness and damping   Q d  1 ( t ) , Q d  2 ( t ) . Let us finally observe that we could have avoided to find the intermediate solution (16), and define the optimization problem (13) in terms of the parametrization   X ( O, L ) ,   and   then   apply   the   procedure explained above to find time evolutions for the constrained gains. Simulations we have performed tend to show that this approach performs worse than the route we propose, and further investigations in this direction are being conducted.  D. Desired matrix correction when two feet balancing  If the constraint (3) acting on the system represents more than one robot frame fixed with respect to the inertial frame (e.g. two feet balancing), the matrices   Q 1   and   Q 2   in (11) may not be full rank. As a matter of fact, the minimal coordinates describing the constrained mechanical system are fewer than  n   in this case. Then, the ranks of the desired matrices   Q d  1  and   Q d  2   must be equal to those of the matrices   Q 1   and   Q 2 . In general, the desired matrices   Q d  1   and   Q d  2   must be corrected according to the constraints acting on the system. To do this, observe that the   feasible   joint accelerations according to the constraints (3) are given by:   ̈ q j   =   − J †  j   ̇ J j    ̇ q j   +   N J    ̈ q j 0 ,   (21) where   J j   = (1 6 n c   −   J b J †  b   ) J j   ,    ̇ J j   is the time derivative of   J j   and   N J   is the projector onto the null space of   J j   . In the above equation, we have used v B   =   − J †  b   J j    ̇ q j   and its derivative. Then, given two desired matrices   Q d  1 , Q d  2 , we project them as follows  Q d  1   =   N J   Q d  1   (22)  Q d  2   =   N J   Q d  2 .  to correct the ranks and structure of the desired stiffness and damping according to the constraints (3) when   n c   >   1 . V. SIMULATIONS RESULTS  A. Simulation Environment  Simulations are performed on a 23 degree of freedom robot model representing the humanoid iCub. The simulation software is developed in MATLAB. The time evolution of the dynamical system is obtained through the integration of the system dynamics (1) subject to the constraints (4). We parametrize the orientation of the base frame using a quater- nion representation   Q ∈   R 4 . The system state is then defined as:   χ   := ( p B ,   Q , q j   ,    ̇ p B , ω B ,    ̇ q j   ) , and the time derivative is given by    ̇ χ   = (  ̇ p B ,    ̇ Q ,    ̇ q j   ,    ̇ ν ) . The state is integrated through time by means of the numerical integrator MATLAB   ode15s . For the purpose of this paper, both problems (16) (20) are solved offline, before starting the state integration. Online implementations of this tuning algorithm will be the subject of a forthcoming publication. To integrate the variables    ̇ L,    ̇ O , we use a fixed step integrator. The constraints (4), as well as   |Q|   = 1   and   OO >   = 1 , need to be enforced during the integration phase, and we added correction terms to    ̇ Q ,    ̇ O . 5   10   15   20 2 4 6 8 10 12 14 16 18 20 22   -10 -5 0 5 10 15 20 25 30  Fig. 1.   Matrix   Q 1   after the gain tuning procedure in case of one foot balancing. Simulations run in MATLAB environment. 5   10   15   20 2 4 6 8 10 12 14 16 18 20 22   -15 -10 -5 0 5 10 15 20 25 30 35  Fig. 2.   Matrix   Q 1   after the gain tuning procedure in case of two feet balancing. Simulations run in MATLAB environment.  B.   Results  Simulations are performed for both the robot balancing on one foot and two feet. We choose   Q d  1   to be positive definite and diagonal, and   Q d  2   = 2 √ Q d  1 . In this case, the desired joint space dynamics aims at the following properties:  •   The joint space dynamics be locally decoupled, i.e. each joint can be tuned separately;  •   The linearized system is critically damped. This will avoid excessive overshoots in the joint space dynamics. When the robot is balancing on two feet, the matrices   Q d  1 , Q d  2  are corrected as in (22). Figures 1-2 show the shape of matrix   Q 1   after gain tuning. Observe that in the case of one foot balancing, the matrix   Q 1  is close to a diagonal matrix, and this implies that the joint space dynamics is almost locally decoupled. In case of two feet balancing, it is interesting to observe the effectiveness of the gain tuning procedure by looking at the difference between the first 11 rows of   Q 1 , and the last 12 rows, which correspond to the closed chained formed by the legs. To verify that the joint space dynamics is close to the desired dynamics, we evaluate the dynamical response of each joint to a step input. In particular, we focused on the settling time   t s . It is possible to approximate   t s   as   t s   ≈  − 3 Re ( λ )   , where   Re( λ )   is the real part of system’s eigenvalues. Figures 3-4 show the dynamics of torso pitch for both one foot and two feet balancing. In both cases,   t s   ≈   t d s   , and the system dynamics is then closed to the desired dynamics.
0   1   2   3   4   5  time [s]  -11 -10.8 -10.6 -10.4 -10.2 -10 -9.8  q j   [deg]  q j d  q j  Desired t   s  Real t   s Fig. 3.   Response of torso pitch to a step input in case of 1 foot balancing. The black dot indicates the desired settling time, while the red dot is the real settling time. The dashed horizontal lines indicate   ± 5%   of the reference position. Simulations run in MATLAB environment. 0   1   2   3   4   5  time [s]  -11 -10.8 -10.6 -10.4 -10.2 -10 -9.8  q j   [deg]  q j d  q j  Desired t   s  Real t   s  Fig. 4.   Response of torso pitch to a step input in case of 2 feet balancing. In this case, the real and desired settling time are almost coincident. Simulations run in MATLAB environment.  VI. CONCLUSIONS This   paper   has   presented   a   gain   tuning   procedure for   constrained   floating   base   systems   controlled   through momentum-based control. The objective is the achievement of a desired local dynamics for the system’s joint space. The optimization is performed on the linearization around an equilibrium point of the closed-loop system’s joint space. The constraints on symmetry and positive definiteness of gain matrices are enforced thanks to a tracker for symmetric positive definite matrices. This allows fast resolutions of the constrained optimization problem, which allows one for on- line implementations of the presented algorithms. Simulation results on the humanoid robot iCub show the effectiveness of the gain tuning procedure for both the robot balancing on one foot and two feet. Further improvements on the gain tuning procedure may be developed in future works. The gains optimization pre- sented in this paper can be applied in different equilibrium points along a joint reference trajectory, and may be an efficient tuning strategy in case of humanoid walking. On line implementations of the presented algorithm on the real humanoid robot is being investigated. APPENDIX  A. Proof of Lemma 1  Recall that   M   is block diagonal. The joint space dynamics is given by the last   n   rows of Eq. (1):  M j    ̈ q j   =   J >  j   f   −   h j   +   τ.   (23) Moreover, we can rewrite the term   JM   − 1 ( h   −   J > f   )   in the control torques equations Eq. (8) as follows:  JM   − 1 ( h   −   J > f   ) =   J b M   − 1  b   ( h b   −   J >  b   f   ) + Λ( h j   −   J >  j   f   )  In view of   N Λ   = 1 n   −   Λ † Λ   and (9), the control torques (8) become:  τ   =   h j   − J >  j   f   +Λ † ( J b M   − 1  b   ( h b   − J >  b   f   ) −    ̇ Jν )+ N Λ u 0   (24) Substituting Eq. (24) into Eq. (23) gives:  M j    ̈ q j   = Λ † ( J b M   − 1  b   ( h b   −   J >  b   f   )   −    ̇ Jν ) +   N Λ u 0 .   (25) The only term which contains the wrenches   f   in Eq. (25) is multiplied by   J >  b   . Since   f   =   f 1   +   N b f 0 , and by definition  J >  b   N b   = 0 6 n c   , we have that   J >  b   f   =   J >  b   f 1 . Hence, vector   f 0  does not influence the joint space dynamics Eq. (25).  B. Proof of Lemma 2  Given two rectangular matrices   A, B , recall the prop- erties   rk ( AB )   ≤   min (rk ( A ) ,   rk ( B ))   and   rk ( B   ⊗   A ) = rk ( A ) rk ( B ) , where   rk ( · )   denotes the rank of a matrix. We apply the above properties to evaluate the rank of the matrices   C 1 , C 2 , C 3 , C 4   in Eq. (11a). It is straightforward to verify that:   rk( C 1 )   ≤   6 ,   rk( C 2 )   ≤   6 ,   rk( C 3 )   ≤   n   −   6 n c ,  rk( C 4 )   ≤   n   −   6 n c . It is now possible to evaluate the rank of matrix  Γ =  [[  C >  2   ⊗   C 1  ]   [  C >  4   ⊗   C 3  ]]  ∈   R n 2 × ( n 2 +36) ,  i.e.  rk(Γ)   ≤   rk( C >  2   ⊗   C 1 ) + rk( C >  4   ⊗   C 3 )   ≤   36 + ( n   −   6 n c ) 2 .  The condition for   Γ   to be full row rank is   rk(Γ) =   n 2 . This condition may be verified if   36+( n − 6 n c ) 2   =   n 2 . Recall that  n, n c   must be positive integers, and that   n >   6 n c . Assume that   n   = 6 n c   +   k , with   k   ∈   N . Then, one can verify that  36 + ( n   −   6 n c ) 2   < n 2   yields   36( n 2  c   −   1) + 12 kn c   >   0 , which is always satisfied for any   n c , k   ∈   N . As a consequence,  rk(Γ)   < n 2 .  C. Proof of Lemma 3  Using Frobenius matrix norms, one has that   |    ̃ K | 2   =  | K ∗   − X | 2   = tr (( K ∗   −   X ) > ( K ∗   −   X )) , where   tr( · )   denote the trace operator. Consider now the candidate Lyapunov function  V   =   |    ̃ K | 2   = tr (( K ∗   −   X ( O, L )) > ( K ∗   −   X ( O, L )))   (26) Observe that   V   is always positive, and   V   = 0   iif    ̃ K   = 0 . Then, to prove the three statements in Lemma 3, it suffices to show that    ̇ V   ≤   0 . To do this, recall that   O   is an orthogonal
matrix, i.e.   OO >   = 1 . Also, observe that Eq. (26) can be rewritten as:  V   = tr ( K ∗ K ∗>   −   2 K ∗> O >   exp( L ) O   + exp(2 L ))   (27) where   we   used   the   properties   tr( K ∗> O >   exp( L ) O )   = tr( O >   exp( L ) OK ∗ )   and   tr( O >   exp(2 L ) O )   = tr( OO >   exp(2 L )) .   To   compute   the   time   derivative   of  V   , recall that    ̇ O   =   OS ( v ) , with   S ( v )   a skew-symmetric matrix, and    ̇ L   =   U   . Then,    ̇ V   becomes:   ̇ V   = 2 tr( B 1   exp( L ) U   ) + 2 tr( B 2 S ( v )) ,  with   B 2   =   O >   exp( L ) OK ∗>   −   K ∗> O >   exp( L ) O , and  B 1   = exp( L )   −   OK ∗ O > . Observe that    ̇ V   ≤   0   if both  tr( B 1   exp( L ) U   )   ≤   0   and   tr( B 2 S ( v ))   ≤   0 . Now, note that  tr( B 1   exp( L ) U   )   can be rewritten as   ∑ n i =1   e >  i   B 1   exp( L ) U e i . Since   the   product   exp( L ) U   is   diagonal,   then ∑ n i =1   e >  i   B 1   exp( L ) U e i   =   ∑ n i =1   e >  i   B 1 e i   exp( l i ) u i , where we   indicate   with   exp( l i ) u i   the   i -th   element   along   the diagonal of   exp( L ) U   . The trace can then be rewritten as ∑ n i =1   e >  i   B 1 e i   exp( l i ) u i   =   ∑ n i =1   b 1 i   exp( l i ) u i   where   b 1 i  is the   i -th element along the diagonal of   B 1 . A possible choice of   u i   that ensures   tr( B 1   exp( L ) U   )   ≤   0   is:  u i   =   − k U i   exp( − l i ) b 1 i   k U i   >   0 .   (28) Since   u i   is the   i -th element along the diagonal of   U   , (28) implies   U   =   − K U   exp( − L ) diag ( B 1 )   with   K U   a diagonal matrix of positive constants. Now, recall that the matrix   B 2   can be decomposed as follows:  B 2   =   ( B 2 + B >  2   ) 2   +   S ( ω )   (29) where   ω   =   S − 1  (   ( B 2 − B >  2   ) 2  )  . Recall also that the trace of a product between a symmetric and a skew-symmetric matrix is zero. Then,   tr  (   ( B 2   + B >  2   ) 2   S ( v )  )  = 0 . We are now left to evaluate   tr( S ( ω ) S ( v )) . The trace of a matrix product can also be written as   tr( X > Y   ) = vec( X ) >   vec( Y   ) , where  vec( · )   is the vectorization operator. Then   tr( S ( ω ) S ( v )) =  −   tr( S ( ω ) > S ( v ))   =   −   vec(( S ( ω )) >   vec( S ( v )) . Note that  vec( S ( x )) =   T x   ∀ x , where the matrix   T   satisfies   T   >   T  2   = 1  due to the skew-symmetry of   S ( · ) . Hence,  tr( S ( ω ) > S ( v )) =   − ω > T   > T v   =   − 2 ω > v  and   this   suggests   that   a   possible   choice   of   v   is  v   =   K v   S − 1 ( S ( ω )) =   K v   S − 1  (   ( B 2 − B >  2   ) 2  )  . R EFERENCES [1]   T. Koolen, S. Bertrand, G. Thomas, T. de Boer, T. Wu, J. Smith, J. Englsberger, and J. Pratt, “Design of a momentum-based control framework and application to the humanoid robot atlas,”   International Journal of Humanoid Robotics , vol. 13, p. 34, March 2016. [2]   M. Hopkins, R. Griffin, A. Leonessa, B. Lattimer, and T. Furukawa, “Design of a compliant bipedal walking controller for the darpa robotics challenge,” in   Humanoid Robots (Humanoids), 2015 IEEE- RAS 15th International Conference on , Nov 2015, pp. 831–837. [3]   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 biped robot combining off-line pattern with real-time modifica- tion,”   Robotics and Automation, 2000. Proceedings. ICRA ’00. IEEE International Conference on , 2000. [4]   J. E. Marsden and T. Ratiu,   Introduction to mechanics and symmetry: a basic exposition of classical mechanical systems .   Springer Science & Business Media, 2013, vol. 17. [5]   R. Featherstone,   Rigid Body Dynamics Algorithms .   Secaucus, NJ, USA: Springer-Verlag New York, Inc., 2007. [6]   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. [7]   P. Wensing and D. Orin, “Generation of dynamic humanoid behaviors through task-space control with conic optimization,” in   Robotics and Automation (ICRA), 2013 IEEE International Conference on , May 2013, pp. 3103–3109. [8]   S. Kuindersma, F. Permenter, and R. Tedrake, “An efficiently solvable quadratic program for stabilizing dynamic locomotion,” in   Robotics and Automation (ICRA), 2014 IEEE International Conference on , May 2014, pp. 2589–2594. [9]   B. Stephens and C. Atkeson, “Dynamic balance force control for compliant humanoid robots,” in   Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on , Oct 2010, pp. 1248– 1255. [10]   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. [11]   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. [12]   E. Farnioli, M. Gabiccini, and A. Bicchi, “Optimal contact force distribution   for   compliant   humanoid   robots   in   whole-body   loco- manipulation tasks,”   2015 IEEE International Conference on Robotics and Automation (ICRA) , pp. 5675–5681, May 2015. [13]   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 [14]   M. Teshnehlab and K. Watanabe, “Self tuning of computed torque gains by using neural networks with flexible structures,”   IEE Pro- ceedings - Control Theory and Applications (Volume:141 , Issue: 4 ) , Feb 2002. [15]   N. Aphiratsakun and M. Parnichkun, “Fuzzy based Gains Tuning of PD controller for joint position control of AIT Leg Exoskeleton- I (ALEX-I),”   Robotics and Biomimetics, 2008. ROBIO 2008. IEEE International Conference on , Feb 2009. [16]   S. Mason, L. Righetti, and S. Schaal, “Full dynamics LQR control of a humanoid robot: An experimental study on balancing and squatting,”  2014 IEEE-RAS International Conference on Humanoid Robots , Nov 2014. [17]   A. Marco, P. Hennig, J. Bohg, S. Schaal, and S. Trimpe, “Automatic LQR tuning based on Gaussian process global optimization,”   2016 IEEE International Conference on Robotics and Automation (ICRA) , May 2016. [18]   D. Xing, C. G. Atkeson, J. Su, and B. J. Stephens, “Gain scheduled control of perturbed standing balance,”   Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on , 2010. [19]   G. Nava, F. Romano, F. Nori, and D. Pucci, “Stability analysis and design of momentum-based controllers for humanoid robots,”   Pro- ceedings of the 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems IROS , Oct 2016. [20]   J. E. Marsden and T. S. Ratiu,   Introduction to Mechanics and Symme- try: A Basic Exposition of Classical Mechanical Systems .   Springer Publishing Company, Incorporated, 2010. [21]   S. Traversaro, D. Pucci, and F. Nori, “On the base frame choice in free-floating mechanical systems and its connection to centroidal dynamics,”   2016.   [Online].   Available:   https://traversaro.github.io/ preprints/changebase.pdf [22]   D. Orin, A. Goswami, and S.-H. Lee, “Centroidal dynamics of a humanoid robot,”   Autonomous Robots , 2013.