^{1}

^{1}

^{1}

^{1}

^{2}

^{1}

^{1}

^{2}

Edited by: Alexandre Bernardino, Universidade de Lisboa, Portugal

Reviewed by: Arnaud Blanchard, University of Cergy Pontoise, France; Oscar Efrain Ramos Ponce, Duke University, USA

This article was submitted to Humanoid Robotics, a section of the journal Frontiers in Robotics and AI.

This is an open-access article distributed under the terms of the Creative Commons Attribution License (CC BY). The use, distribution or reproduction in other forums is permitted, provided the original author(s) or licensor are credited and that the original publication in this journal is cited, in accordance with accepted academic practice. No use, distribution or reproduction is permitted which does not comply with these terms.

This paper details the implementation of state-of-the-art whole-body control algorithms on the humanoid robot iCub. We regulate the forces between the robot and its surrounding environment to stabilize a desired posture. We assume that the forces and torques are exerted on rigid contacts. The validity of this assumption is guaranteed by constraining the contact forces and torques, e.g., the contact forces must belong to the associated friction cones. The implementation of this control strategy requires the estimation of both joint torques and external forces acting on the robot. We then detail algorithms to obtain these estimates when using a robot with an iCub-like sensor set, i.e., distributed six-axis force-torque sensors and whole-body tactile sensors. A general theory for identifying the robot inertial parameters is also presented. From an actuation standpoint, we show how to implement a joint-torque control in the case of DC brushless motors. In addition, the coupling mechanism of the iCub torso is investigated. The soundness of the entire control architecture is validated in a real scenario involving the robot iCub balancing and making contact with both arms.

Classical industrial applications employ robots with limited mobility. Consequently, assuming that the robot is firmly attached to the ground, interaction control (e.g., manipulation) is usually achieved separately from whole-body posture control (e.g., balancing). Foreseen applications involve robots with augmented autonomy and physical mobility. Within this novel context, physical interaction influences stability and balance. To allow robots to overcome barriers between interaction and posture control, forthcoming robotics research needs to investigate the principles governing

It is worth recalling that the aforementioned industrial robots have been extensively studied since the early seventies. Robot physical autonomy asks for switching from conventional fixed-base to free-floating robots, whose control has been addressed only during recent years. Free-floating mechanical systems are under actuated and therefore cannot be fully feedback linearized (Spong,

The major contribution of this work is the implementation and integration of all the building blocks composing a system for balance and motion control of a humanoid robot. The system includes the low-level joint-torque control, the task-space inverse-dynamics control, the task planner and the estimation of contact forces and joint torques. Even though in recent years, other similar systems have been presented (Ott et al.,

differently from the other robots, iCub can localize and estimate contact forces on its whole-body thanks to its distributed tactile sensors

similarly to the

differently from the above-mentioned platforms, iCub is not equipped with joint-torque sensors, but we designed a method that exploits its internal 6-axis force/torque sensors to estimate the joint torques

all our control loops run at 100 Hz, which is (at least) 10 times slower with respect to Ott et al. (

We believe that further investigation will be necessary to thoroughly understand all the consequences of our hardware/software design choices. Nonetheless, these peculiarities make the presented system unique, and for this reason we think it is important to share our results with the robotics community.

The paper is organized as follows: Section

This section reviews previous literature on rigid contacts and their role in whole-body stability. Then we conclude that for the scope of the current paper we need to consider local contact stability as opposed to global stability criteria proposed in previous literature. Section

We consider articulated rigid-body systems under the effects of multiple rigid contacts. In general, a contact can be seen as a continuum of infinitesimal forces acting on the surface of a rigid body. The effect of contact forces will be represented with an equivalent wrench _{c}_{c}_{c}_{c}_{c}_{c}_{c}_{o}_{o}_{o}_{o}_{o}

Given a rigid body subject to contact forces, we associate a field of pressure to the contact itself. For each contact point, the pressure is defined as the amount of normal force acting per unit area. The center of pressure (CoP) is defined as an application point where the force obtained by the integration of the field of pressures causes an effect that is equivalent to that of the field of pressures itself.

Given the above definition, we can infer that the CoP sometimes might not exist. As a trivial example, the CoP does not exist when a field of pressure generates a zero net force but a non-zero net torque. Excluding this trivial case, by resorting to the Poinsot theorem it can be shown (see Appendix

A particular type of contacts, nominally planar unilateral contacts, has been widely studied to characterize the stability of an articulated rigid-body system while walking on flat terrain. The typical case-study considers a single link (foot) in contact with a flat surface (ground). Proposed stability criteria take into account the fact that while the foot has to be constantly in contact with the ground, the rest of the body is moving and therefore transfers inertial and gravitational forces to the foot. The foot is therefore subject to two sets of wrenches: those due to the contact with the ground (_{c}_{o}_{c}_{c}

So far, we have only discussed about forces generated by contacts. In general, contacts also introduce motion constraints and there is always a duality between contact forces and constrained motion. In the case of rigid contacts, the directions in which motion is constrained are precisely those in which (contact) forces can be exerted as observed by Murray et al. (

In the case of planar unilateral contacts, these conditions assume an elegant form that will be presented in Section ^{1}_{c}_{o}_{c}_{o}_{o}_{o}_{c}_{c}_{o}_{o}

Given a planar unilateral contact, the set of forces induced by the contact are such that it is always possible to find a point on the plane where the equivalent moment has null tangential components. This point has been named zero moment point (ZMP) by Vukobratovic and Juricic (

Goswami (_{o}_{o}_{o}_{o}^{2}

Both the ZMP and the FRI concept have been used by several authors to define a suitable stability margin for balancing an articulated rigid body system. Hirai et al. (

So far, we discussed the stability of an articulated rigid body system subject to a single planar unidirectional contact. The stability characterization given by Goswami (

In this section, we consider the case of articulated rigid body systems in contact with a flat surface (typically the ground). Differently from the previous sections, we assume that more than one single rigid body is in contact with the flat ground and therefore we consider a multiple coplanar contacts scenario. Within this context, we distinguish between local CoP (one per each rigid body in contact with the ground) and global CoP (the center of pressure resulting from all rigid bodies in contact). The local CoP of a rigid body has been defined and characterized in Sections

Harada et al. (

In the present paper, we formulate a whole-body postural control, which assumes stable contacts. Stability, as defined in Section

We now briefly review the vast literature on prioritized task-space inverse-dynamics control and we motivate our choices in this regard. Sentis (

As opposed to these analytical solutions to the control problem, an alternative numerical approach proposed by de Lasa et al. (

We decided to take an in-between approach: our framework of choice [see Section

In this section, we present our approach to solve the problem of controlling whole-body posture on multiple rigid planar contacts. We suppose each contact to be planar, but contact planes to be in general non-coplanar. Within this context, the considerations presented in the previous sections justify our choice to abandon the idea of defining a global stability criterion (such as the GCoP). In case of non-coplanar contacts, a global CoP is not even properly defined given that the resulting force and torque might not in general be orthogonal (see Sections

In the rigid and planar contact case, contact stability has been characterized by means of the contact FRI and CoP. Both quantities depend on the wrench (i.e., both force and torque) at the contact point. Assuming that contacts might occur at any point on the robot body, an estimate of the contact wrench might be difficult to obtain if not impossible. Conventional manipulators measure wrenches at the end-effector, where force and torque sensors are placed. Joint-torque sensing gives only an incomplete characterization of contact wrenches. The problem can be solved adopting whole-body distributed force/torque (F/T) and tactile sensors as those integrated in the iCub humanoid (Section

The platform used to perform experimental tests is the iCub humanoid robot, which is extensively described in Metta et al. (

Fumagalli et al. (

We now describe a method for the estimation of contact wrenches; more details can be found in Del Prete et al. (_{i}_{i}_{0} (i.e., the F/T sensor measurement), the _{0,ei}, … , _{0,eK} (i.e., the locations where the skin senses contacts), and we want to estimate the _{e1}, … , _{eK}_{i}^{u}^{6×}^{u}^{6} are completely determined. The equations are constructed taking into account the type of possible contacts among the following three: pure wrench (_{e}_{e}_{e}_{w}_{f}_{n}^{3×3} is the skew-symmetric matrix such that _{en}

The vector ^{†} is the Moore–Penrose pseudo-inverse of ^{3}

Once an estimate of external forces is obtained with the method described in Section

As it was previously pointed out, the technology available in the iCub (nominally, whole-body distributed tactile and F/T sensing) and the estimation algorithm presented in the previous sections allows to simultaneously estimate internal (i.e., joint torques) and external (i.e., contact) forces. The accuracy of the estimates is decisive for the efficacy of the control algorithm that will be described in Section

The accuracy of the system dynamics equation (7) is crucial in the proposed control framework since it affects the controller equation (8) and the internal/external torque estimation procedure described in Section

Another important component in implementing the control strategy detailed in Section _{i} is proportional (_{t}_{i}_{v}_{c}_{t}_{vp}_{vn}_{cp}_{cn}^{4}_{s}_{p}_{i}

Specific care was posed in controlling the torques at joints actuated with a differential mechanism. As an example, we consider here the torso roll, pitch, and yaw joint represented in Figure ^{3} the vector of the joint angles corresponding to the torso yaw, the torso roll, and torso pitch degree of freedom. Also, by abusing notation, define ^{3} as the angles between the stator and the rotor of the motors 0B4M0, 0B3M0, and 0B3M1. Then, a simple analysis leads to the following relationship:
_{q} to be the link torques and _{θ}

^{3}) and joint (^{3}) positions are coupled as described in Figure

_{2} implies an equal and opposite movement of the rotor of the motor 0B4M0. This rotor moves, modulo the transmission ratio, also when a yaw movement of an angle _{1} occurs.

In the case of coupled joints, the transformation matrix ^{5}_{θ}_{θ}_{q}) as follows:

In this section, we describe the control algorithm for controlling an articulated rigid body subject to multiple rigid constraints. The system dynamics are described by the following constrained differential equations:
^{n}_{j}^{n}^{+6} represents the robot velocity (it includes both _{b}^{6}), ^{n}^{(}^{n}^{+6)×(}^{n}^{+6)} is the mass matrix, ^{n}^{+6} contains both gravitational and Coriolis terms, ^{n}^{×(}^{n}^{+6)} is the matrix selecting the actuated degrees of freedom, ^{k}_{c}_{c}_{c}^{k}^{×(}^{n}^{+6)} is the contact Jacobian. Let us first recall how the force-control problem is solved in the task space inverse dynamics (TSID) framework proposed by Del Prete (^{k}_{i}_{i}

Assuming that the force task has maximum priority the solution is:
^{6×(}^{n}^{+6)} is the matrix selecting the floating-base variables, and the algorithm is initialized setting _{p}_{(}_{N}_{)} = _{N}_{c}

The final validation of the proposed control framework requires the definition of a suitable set of position (_{B}_{B}^{k}X_{i}_{j}_{i}_{i}

^{q}_{j}

The set of tasks active at a certain instant of time is regulated by a finite state machine. In particular, there are five different states _{1}, …, _{5} each characterized by a different set of active tasks _{1}, …, _{5}.

_{1} has the following set of active tasks _{1} = {^{q}

_{2} has the following set of active tasks

_{3} has the following set of active tasks

_{4} has the following set of active tasks

_{5} has the following set of active tasks

Transition between states is regulated by the following finite state machine where the sets _{ra}_{la}

In practice, at start (_{0}_{1} in order to maintain a configuration, which is as close as possible to the initial configuration with a postural task (^{q}_{1}_{2} by adding two tasks to the set of active tasks: a control of the forces exchanged by the left and right foot (_{ra}_{ra}_{2}. A transition from _{2} to _{3} is performed when the system detects a contact on the right arm (_{ra}_{2} with the addition of the task responsible for controlling the force at the right arm contact location (_{2} to _{4} is performed when the system detects a contact on the left arm (_{la}_{2} with the addition of a task responsible for controlling the force at the left arm contact location (_{3} or _{4} to _{5} is performed whenever the robot perceives a contact so that in this new situation both arms are in contact (_{ra}_{la}_{5}, both arms are used to control the interaction forces by activating the tasks

In this section, we discuss how to compute the task references:
_{com}_{com}_{1}, …, _{5} the meaning of

In the different states, the following equations on _{g}

In literature, these equations (derived from the Newton–Euler equations) have been presented in detail by Orin et al. (_{i}:
_{W}^{⊤}

This solution gives a set of desired forces _{0} and weight matrix _{0} to have a null torque component penalizes solutions whose FRI is closer to the polygon borders and this penalty monotonically increases with the distance from the center of the maximum circle. Similarly, friction cones constraints are enforced by choosing the components of _{0} to be sufficiently far from the cone borders. Assuming contact plane normals to coincide with the _{0}. Otherwise a viable choice is also to choose _{0} = 0 since in most realistic situations the solution _{g}

We implemented the proposed control strategy on the iCub humanoid. In a first phase, the iCub was balancing with both feet on the ground plane (coplanar flat contacts, see Figure _{com}_{0}) along the robot transverse axis (_{r}^{6}

In a second phase, the iCub was maintaining its center of mass at its initial position _{d}_{ra}_{d}_{la}_{d}

The present paper addressed the problem of whole-body motion control in the presence of multiple non-coplanar and rigid contacts. The proposed solution defines a stability criterion based on the FRI of individual contacts as opposed to global stability criteria. It is argued that the FRI lying on the contact support polygon is a necessary and sufficient condition for the contacts to impose always the same motion constraints on the whole-body dynamics. These stability conditions are therefore the ones adopted in this paper in order to avoid the complications of hybrid and switching systems control. The chosen stability conditions require the capability of simultaneously measuring forces and torques (i.e., wrenches) at any possible contact location. This is not possible with conventional torque-controlled manipulators and requires whole-body distributed force and tactile sensing. These sensing capabilities are available in the iCub humanoid exploiting its whole-body distributed artificial skin and force/torque sensors. In consideration of this specific hardware, in the present paper, we discussed our approach to obtain: contact-wrench estimates, internal-torque measurement and dynamic model identification. All these components are functional to the implementation of the proposed whole-body controller with multiple non-coplanar contacts.

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

The Poinsot theorem [see p. 65 in Murray et al. (

The proof of the equivalence between

Besides being an equivalent wrench, the Poinsot wrench _{QL}

Therefore, _{QL}

On the Poinsot axis, the torque norm is minimal and equals:

Given a rigid body subject to a field of pressures, the center of pressure (CoP) is an application point where the equivalent torque (due to the field of pressure) is null. Being the Poinsot axis, the geometric locus of minimum torques, it is evident that a CoP can be defined if and only if _{c}_{c}

In this section, we provide a counterexample to show that the condition on GCoP (global center of pressure) to lie in the contacts convex hull is not sufficient to guarantee the stability of individual contacts. Consider the simple system represented in Figure

_{1}, _{2}, and _{3}. _{r}_{l}^{2}, _{r}_{l}_{1}, _{2}, _{3} ∈ ℝ^{2}.

In this section, we provide explicit computation for the zero tipping moment point associated to a wrench _{c}_{c}_{c}_{c}_{c}_{c}_{x}_{y}_{PP′}_{c}_{c}_{c}

Assuming that the contact wrench _{c}_{c}_{c}_{c}

Therefore, we have:
_{c}

In the following paragraphs, we study equilibrium configurations for the system in Figure _{r}_{l}^{2}, _{r}_{l}_{1}, _{2}, _{3} ∈ ℝ^{2}) forces and torques give twelve unknowns that can be uniquely solved for any choice of the joint torques _{1}, _{2}, _{3}. To simplify the notation and to obtain a symmetric solution we assume _{1} = _{2} = 0, _{1} = −

Assuming for the sake of simpler notation _{1} + _{2} = _{3} + _{4} = _{tot}^{1}_{r}_{l}

The global center of pressure (GCoP) can be computed by representing _{r}_{l}_{r}_{l}_{tot}_{tot}^{T}. As expected by the system symmetry, the global center of pressure is always in the middle of the two contacts regardless of the value given to _{r}_{l}

If _{l}_{r}_{r}_{r}_{l}_{l}

In this section, we make the hypothesis that contact constraints force the left foot to rotate around its right edge and the right foot around its left edge. In this specific case, the torques at the (point wise) contacts is identically null. With respect to the previous situation we therefore have two unknowns less. Additional unknowns come from the fact that we are no longer assuming accelerations to be identically null since the system is no longer assumed at equilibrium. These additional unknowns can be expressed as a function of only two unknowns (e.g., the left and right foot tipping accelerations). This results by taking into consideration the kinematic constraints in the system, as represented in the left hand side of Figure ^{2}_{l}_{1} and _{1} reprojected on the planar contact surface. Similar considerations hold for FRI_{r}_{1} and _{2} are positive scalars which again depend only on the system geometric and dynamic parameters. As expected FRI_{r}_{l}_{3} and _{4}. Contact forces are therefore positive as expected given the unilaterally of the contact. Finally, given the symmetry of the problem, the GCoP is constantly at the center of symmetry of the system and therefore within the contact support polygon. Therefore, when

^{1}Using the FRI definition and assuming the contact plane to be

^{2}

The authors acknowledge Ali Paikan (Istituto Italiano di Tecnologia, iCub Facility) for his contribution on the Lua bindings, Daniele Domenichelli (Istituto Italiano di Tecnologia, iCub Facility) for the codyco-superbuild support, Lorenzo Natale (Istituto Italiano di Tecnologia, iCub Facility) for the support to the YARP framework, Marco Randazzo (Istituto Italiano di Tecnologia, iCub Facility) for the implementation of torque control on the iCub humanoid, Julien Jenvrin (Istituto Italiano di Tecnologia, iCub Facility) for the technical hardware support, and Giorgio Metta for the coordination of the iCub Facility activities.

^{1}Normal linear motion and tangential angular motions are constrained by the unilateral contact forces; tangential linear motions and normal angular motion are constrained by friction.

^{2}The FRI and ZMP (as they have been defined) do not depend neither on the tangential forces nor on the normal moments. However, frictional constraints depend on these quantities. Therefore, no stability criteria can be formulated using only the FRI and the ZMP quantities. Within this context, it comes with no surprise that the sufficient condition for contact stability requires tangential forces and normal moments to be within the friction cones of the contact itself.

^{3}See the software library documentation

^{4}

^{5}Both the identification equation (

^{6}