Whole-body Force Control of the iCub Humanoid Robot for Balancing Tasks Francesco Romano and Daniele Pucci and Silvio Traversaro and Jorhabib Eljaik and Francesco Nori Abstract— This paper overviews the whole-body force control framework implemented on the iCub humanoid robot. This framework consists in a hierarchy of two tasks, where the highest priority task is a force control task, and the lowest one a postural task. Forces are regulated to stabilize a desired cen- troidal dynamics, i.e., a desired linear and angular momentum of the rigid body system. In turn, these forces are generated by the internal links’ torques, which are related to forces through the rigid constraint equations and the free-floating dynamics. Validation of the framework has been conducted on a real scenario involving the iCub robot balancing while subject to additional external forces. I. I NTRODUCTION Forthcoming robotic applications require robots to phys- ically interact with the environment. This requirement is different from those of classical industrial applications where robots are primarily required to perform positioning tasks. Another key element for innovative robotics is the need of increased mobility and adaptation to human-centered environment, e.g. walking, while performing interaction tasks. Within this context, physical interaction influences stability and balance. To allow proper coordination between interaction and posture control, robotics research needs to investigate the principles governing whole-body control with contact dynamics, as these represent important challenges towards the achievement of robots with augmented physical autonomy and will therefore be the focus of the present paper. It is worth to recall that industrial robots have been extensively studied since the early seventies. These robots are often fixed-base, and usually confined in a protected and well-known environment. On the contrary, robots with augmented mobility ask for switching from the conventional fixed-base theoretical framework to free-floating approaches, whose control has been addressed only during recent years. Full feedback linearization of free-floating mechanical sys- tems is forbidden due to their underactuated nature [1] thus prohibiting the application of classical methodologies. The problem becomes even more complex when these systems are constrained, that is their dynamics are subject to a set of (possibly time-varying) nonlinear constraints. This is the typical case for legged robots for which motion is constrained by rigid contacts with the ground. *This work was supported by the FP7 EU projects CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics), Koroibot (No. 611909 ICT- 2013.2.1 Cognitive Systems and Robotics). The authors are with the Istituto Italiano di Tecnologia. RBCS depart- ment. Via Morego, 30. 16013 Genova - Italy name.surname@iit.it This paper presents the control framework implemented on the humanoid robot iCub for balance and motion control. II. CONTROL FRAMEWORK A. Whole-Body Force Control In this section we describe the control algorithm for controlling an articulated rigid body subject to multiple rigid constraints. The system dynamics are characterized by the following differential equations: M (q)˙ ν + h(q,ν ) − J ⊤ (q)f = Sτ , (1a) J (q)˙ ν + ˙ J (q,ν )ν = 0, (1b) where q ∈ SE(3) × R n represents the configuration of the free floating system, which is given by the pose of a base- frame and n generalized coordinates q j characterizing the joint angles. The vector ν ∈ R n+6 represents the robot velocity (it includes both ˙ q j ∈ R n and the linear and angular velocity of the base-frame v b ∈ R 6 ), the system acceleration is denoted as ˙ ν , the derivative of ν , the control input τ ∈ R n is the vector of joint torques, M ∈ R (n+6)×(n+6) is the mass matrix, h ∈ R n+6 contains both gravitational and Coriolis terms, S ∈ R n×(n+6) := (0 n×6 ,I n ) ⊤ is the matrix selecting the actuated degrees of freedom, f ∈ R 6 is the external, contact wrench, and J ∈ R 6×(n+6) is the contact Jacobian. Let us briefly recall how the force-control problem is solved in the Task Space Inverse Dynamics (TSID) frame- work proposed by [2] in the context of free floating robots. The framework computes the joint torques to match as close as possible a desired vector of forces at the contacts (2a) while being compatible with the system dynamics (2b) and contact constraints (2c): τ ∗ = argmin τ ∈R n ∥f − f ∗ ∥ 2 (2a) s.t. M ˙ ν + h − J ⊤ c f = Sτ (2b) J c ˙ ν + ˙ J c ν =0 (2c) where f ∗ ∈ R k is the desired value for the contact forces. Then we can exploit the null space of the force task to perform lower priority motion tasks. These tasks (indexed with i) are all expressed as the problem of tracking a given reference acceleration ˙ ν ∗ i for a variable x i differentially linked to q by the Jacobian J i . We impose that the force task has maximum priority in the task hierarchy. The final solution, i.e. the torques to be applied to the robot, are obtained by recursively solving a minimization problem