Modeling and Control of Humanoid Robots 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 robot 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 robotics 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 humanoid robot 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, Genoa, 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 2 R ;Mj 2 R , H = (HL ;H! ) 2 R the robot centroidal momentum, and H ;H 2 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 2 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 2 R is the velocity of masses of the robot and the seesaw, respectively. 3 3 3×3 the system’s center of mass p 2 , and ! 2 is the • We denote with S(x) 2 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 y n×m on a link remains constant w.r.t. the inertial frame, i.e. the • Given a matrix A 2 R , we denote with A 2 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 2 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 2 3 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, 4 5 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 > 2 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) 2 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) 2 R , while f = 6nc as in [17, Ch. 13.5] results in the following equations of (f1; ··· ; fnc ) 2 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 2 R(n+6)×(n+6) is the mass matrix, C 2 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 2 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, τ 2 Rn is a centroidal momentum, while the secondary task is to stabilize vector representing the internal actuation torques, and fk 2 the system’s zero dynamics.