HAL Id: hal-01137239
https://hal.archives-ouvertes.fr/hal-01137239
Submitted on 30 Mar 2015
HAL is a multi-disciplinary open access
archive for the deposit and dissemination of
sci-entific research documents, whether they are
pub-lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.
L’archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d’enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.
rigid non-coplanar contacts
Francesco Nori, Silvio Traversaro, Jorhabib Eljaik, Francesco Romano,
Andrea del Prete, Daniele Pucci
To cite this version:
Francesco Nori, Silvio Traversaro, Jorhabib Eljaik, Francesco Romano, Andrea del Prete, et al.. iCub
whole-body control through force regulation on rigid non-coplanar contacts. Frontiers in Robotics and
AI, Frontiers Media S.A., 2015, pp.18. �10.3389/frobt.2015.00006�. �hal-01137239�
iCub whole-body control through force regulation on rigid
non-coplanar contacts
Francesco Nori1*, Silvio Traversaro1, Jorhabib Eljaik1, Francesco Romano1, Andrea Del Prete2and Daniele Pucci1
1
Cognitive Humanoids Laboratory, Department of Robotics Brain and Cognitive Sciences, Fondazione Istituto Italiano di Tecnologia, Genoa, Italy 2
CNRS, LAAS, University of Toulouse, Toulouse, France
Edited by:
Alexandre Bernardino, Universidade de Lisboa, Portugal
Reviewed by:
Arnaud Blanchard, University of Cergy Pontoise, France
Oscar Efrain Ramos Ponce, Duke University, USA
*Correspondence:
Francesco Nori , Cognitive Humanoids Laboratory, Department of Robotics Brain and Cognitive Sciences, Fondazione Istituto Italiano di Tecnologia, Via Morego 30, Genova 16163, Italy
e-mail: francesco.nori@iit.it
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.
Keywords: whole-body control, floating-base robots, rigid contacts, non-coplanar contact, tactile sensors, force sensors
1. INTRODUCTION
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 usu-ally achieved separately from whole-body posture control (e.g., balancing). Foreseen applications involve robots with augmented autonomy and physical mobility. Within this novel context, phys-ical 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 whole-body coordination with contact dynamics, as these represent important challenges toward achieving robot physical autonomy and will therefore be the focus of the present paper.
It is worth recalling that the aforementioned industrial robots have been extensively studied since the early seventies. Robot phys-ical 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, 1994). The problem becomes even more complex when these systems are constrained that is their dynamics are subject to a set of (possibly time-varying) non-linear constraints. This is the typical case for legged robots, for which motion is constrained by rigid contacts with the ground.
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 sys-tem 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., 2011;Herzog et al., 2014), the originality of our contribution lies (i) in the specificities of our test platform and (ii) in a number of design choices that traded off simplicity of implementation for performances of the control system. In particular:
• 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 DLR-Biped (Ott et al., 2011), iCub is actuated with DC motors and harmonic drives, but we chose to neglect the gear-box flexibility, which simplified the motor-identification procedure and the low-level torque controller
• 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 toOtt et al. (2011)andHerzog et al. (2014). We believe that further investigation will be necessary to thor-oughly 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 2 reviews the state of the art and motivates our specific choices, with a focus on why we defined postural stability by means of the center of pressure at individual contacts. A counterexample discussed in Section 2.3.3 shows that the commonly used global center of pressure is not suitable for the scope of our application. Section 3.1 describes the whole-body distributed force and tactile sensors on iCub. These sensors are used to estimate contact forces (Section 3.2.1), internal torques (Section 3.2.2), and to improve the accuracy of the robot’s inertial parameters (Section 3.3), while Section 3.4 presents the prioritized contact-force controller. Section 4 discusses the imple-mentation scenario, consisting in controlling the iCub posture and contact forces at both arms and feet. Remarkably, iCub can estab-lish and break contacts at the arms using tactile sensing for both contact detection and localization. Finally, Section 5 draws the conclusions.
2. BACKGROUND
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 2.1 makes some general considerations about contacts and Section 2.1.1 gives a characterization of contacts by means of the center of pressure, a point in space that summarizes the effects of distributed forces acting on a rigid body. Section 2.2 focuses on planar unilateral contacts and their stability (Section 2.2.1). This specific type of contacts is associated with a center of pressure that lies on the contact plane (Section 2.2.2). This property is exploited to give necessary and sufficient conditions for the stability of a pla-nar unilateral contact (Section 2.2.3). Section 2.3 reviews previous literature on multiple contacts. In particular, Section 2.3.1 consid-ers the coplanar case, whereas Section 2.3.2 the non-coplanar one. Section 2.3.3 discusses a counterexample to justify our choice of addressing the multiple-contact case without resorting to a global stability criterion as proposed in most of previous approaches. Finally, Section 2.4 briefly reviews the state of the art of prioritized task-space inverse dynamics.
2.1. CONTACTS
We consider articulated rigid-body systems under the effects of multiple rigid contacts. In general, a contact can be seen as a con-tinuum of infinitesimal forces acting on the surface of a rigid body. The effect of contact forces will be represented with an equivalent wrench wc=(fc,µc), composed by a three-dimensional force and a three-dimensional torque, denoted fcandµc, respectively. Con-sidering that a contact exerts infinitesimal forces distributed over a surface, fcis computed as the integral of infinitesimal forces over the surface. Similarly,µcis computed as the integral of the infin-itesimal torques due to infininfin-itesimal forces over the surface. The effect of other (non-contact) forces and torques acting on the rigid body will be denoted as wo=(fo,µo), being foandµothe equiva-lent force and torque (respectively) resulting from all non-contact forces.
2.1.1. Definition of center of pressure (CoP)
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.
Remark 1: by definition, pressure accounts only for the normal
component of the contact forces acting on the surface of a rigid body. Therefore, the CoP comes handy especially when the tan-gential forces (shear stress) are negligible or does not play a role in the rigid body dynamics (e.g., the effect of tangential forces is compensated by the static friction). Otherwise, the CoP does not represent per se a full characterization of the forces acting on the system and the effect of tangential forces should be also taken into account. Remark 2: at any point in space, the effect of a field of pressures can be represented by an equivalent force and an equivalent torque (the integral of infinitesimal forces and torques, respectively). Given the above definition, the CoP is an application point where the equivalent torque is null.
2.1.2. Existence of the center of pressure (CoP)
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 the-orem it can be shown (see Appendix A) that the CoP is defined if and only if the resulting net torque is orthogonal to the result-ing net force. Two relevant cases that satisfy this condition can be identified: field of pressures due to forces lying on a plane (torques orthogonal to the plane) and field of pressures due to forces orthogonal to a plane (torques lying on the plane). The first is a typical example used in aerodynamics (profile of a wing) and the latter is a typical example in the field of humanoid walk-ing (contact with planar surfaces). In any case, the CoP is never uniquely defined and the set of valid CoPs corresponds to the Poinsot axis (see Appendix A.2). In the case of planar contacts, we will uniquely identify the CoP with the intersection between the axis and the planar contact surface as proposed bySardain and Bessonnet (2004).
2.2. PLANAR UNILATERAL CONTACT
A particular type of contacts, nominally planar unilateral con-tacts, 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 (wc) and those due to the movements of the rest of the body (wo). For the contact to be stable, these forces should balance (see Section 2.2.1). Force balance might not always hold since planar unilateral contacts exert a limited range of forces and torques. Original stability properties were proposed by Vukobra-tovic and Juricic (1969), who introduced the zero moment point (ZMP) concept. The ZMP coincides with the unique point on the ground where fc,µcproduce zero tangential moments (see Section 2.2.2). As it was pointed out bySardain and Bessonnet (2004), in
the case of planar contacts, the ZMP coincides with the intersec-tion of the Poinsot axis with the contact plane as defined in Secintersec-tion 2.1.1; the ZMP is therefore a valid CoP. Other stability criteria for planar unilateral contacts have been proposed byGoswami (1999)
and reviewed in Section 2.2.3.
2.2.1. Definition of stable planar unilateral contact
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 con-strained are precisely those in which (contact) forces can be exerted as observed byMurray et al. (1994). In other words, contact forces and possible motions are always orthogonal. From a control point of view, it is often desirable that the set of motion constraints does not change over time, since if it does, the control problem becomes harder to solve (see, for example, literature on hybrid and switch-ing systems). This is the reason why we say that a contact is stable
when the motion constraints induced by it do not change over time.
Interestingly, motion constraints are effective on the system only if certain conditions are satisfied. If these conditions are not met (e.g., contact forces violate unilateral constraints or exceed friction cones) contacts are broken and motion constraints are no longer active on the system. It is therefore important to find a set of nec-essary and sufficient conditions for a contact to constrain always the same motion.
In the case of planar unilateral contacts, these conditions assume an elegant form that will be presented in Section 2.2.3. The analysis is simplified by observing that planar unilateral contacts impose constraints on all linear and angular motions1. There-fore, a planar unilateral contact imposes null linear and angular accelerations. By means of the Newton–Euler equations on the contact link, motion constraints are therefore guaranteed if and only if fc= −fo andµc= −µo. In a sense, deciding whether or not a planar unilateral contact is stable (with the above terminol-ogy) corresponds to understanding if a given wrench fo,µocan be compensated by the forces fcand torquesµcgenerated by the given type of contact. If −fo, −µolie outside the space of wrenches that a planar unilateral contact can generate, then non-zero accel-erations are generated and the contact is broken. The following section shows how to characterize the set of wrenches generated by a planar unilateral contact.
2.2.2. Characterization of the CoP for planar unilateral contacts 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 compo-nents. This point has been named zero moment point (ZMP) by
Vukobratovic and Juricic (1969). The name is sometimes consid-ered misleading [see, for example,Sardain and Bessonnet (2004)] since at the ZMP the “tipping” (or tangential) moment and not the “total” moment is zeroed. The computation of the zero tip-ping moment point is relatively straightforward and reformulated
1Normal linear motion and tangential angular motions are constrained by the
uni-lateral contact forces; tangential linear motions and normal angular motion are constrained by friction.
in Section “Appendix B.1.” In the case of unilateral contacts, the ZMP coincides with the CoP [seeSardain and Bessonnet (2004)] and always lies in the convex hull of the contact points as shown byWieber (2002)and in Section “Appendix B.1.” It is worth notic-ing that (in the ZMP context) restrictnotic-ing to tangential moments corresponds (in the CoP context) to neglecting tangential forces (see the first remark in Section 2.1.1).
2.2.3. Characterization of stable planar unilateral contact
Goswami (1999)pointed out that the ZMP lying within the con-tact convex hull is not a proper stability measure. He therefore formulated some different statements for the characterization of the stability of planar unilateral contacts. These statements make use of the foot rotation indicator (FRI), which corresponds to the unique zero tipping moment point associated to fo,µoand belonging to the contact plane. The name FRI is misleading since this physical quantity can be associated to any rigid body in contact with a planar surface, regardless of the fact that the body itself is a foot or not. Remarkably, the FRI (differently form the CoP) is not constrained in the contact convex hull because foandµoare not the result of unilateral contact forces. Other names used in liter-ature for the FRI are fictitious ZMP (FZMP) and computed ZMP (CZMP) used byVukobratovic and Borovac (2004)andKajita and Espiau (2008), respectively. If the FRI is not within the contact con-vex hull,Goswami (1999)has shown that a rotation of the rigid link is occurring (i.e., angular acceleration is not identically zero). Vice versa, if the FRI is within the contact convex hull and if fric-tional constraints are satisfied2, then the unilateral contact is stable (i.e., the contact link has null accelerations). The complete proof of the latter statement requires some additional considerations, which are outside the scope of the present paper. The interested reader should refer to chapter 11 ofFeatherstone (2008).
Both the ZMP and the FRI concept have been used by sev-eral authors to define a suitable stability margin for balancing an articulated rigid body system.Hirai et al. (1998)used the ZMP concept to balance one of the earliest versions of the Honda walk-ing humanoids. Huang et al. (2001)adopted a similar concept to define a stability margin tunable by modifying the robot hip motion.Li et al. (1993)used the error between a desired and the computed ZMP to learn stable walking. A good reason to prefer the FRI has been pointed out byGoswami (1999): given that “the ZMP cannot distinguish between the marginal state of static equi-librium and a complete loss of equiequi-librium of the foot (in both cases, the ZMP is situated at the support boundary), its utility in gait planning is limited. FRI point, on the other hand, may exit the physical boundary of the support polygon and it does so whenever the foot is subjected to a net rotational moment.”
2.3. MULTIPLE CONTACTS
So far, we discussed the stability of an articulated rigid body sys-tem subject to a single planar unidirectional contact. The stability
2The FRI and ZMP (as they have been defined) do not depend neither on the
tan-gential 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.
characterization given by Goswami (1999) guarantees contacts stability by requiring the FRI to stay inside the support convex hull. In this section, we review extensions of this criterium to the case of multiple contacts. Proposed extensions search for some global stability criteria to condense the local stability criteria on individual contacts. Section 2.3.1 considers the simple case of mul-tiple coplanar contacts and the associated global stability criteria, known as global CoP. Section 2.3.2 reviews previous literature on global criteria with multiple non-coplanar contacts. The present section concludes by observing that, for the scope of the present paper, it is mandatory to abandon global criteria and stick to local ones. The conclusion follows from a counterexample, provided in Section “Appendix A.2” and discussed in Section 2.3.3.
2.3.1. Multiple coplanar contacts
In this section, we consider the case of articulated rigid body systems in contact with a flat surface (typically the ground). Dif-ferently 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 con-text, 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 2.1.1, 2.2.2, and 2.2.3. These definitions and characterizations refer to a single rigid body (e.g., the foot) but can be extended to any rigid body of the articulated system. Computations in this case account only for the contact forces acting on the rigid body itself. Global CoP (GCoP) is instead a quantity associated with the whole articulated system and corresponds to the center of pressure obtained by inte-grating all contact forces acting on the articulated system. Most of the previous literature does not distinguish between local and global center of pressure but often refer to the latter when charac-terizing stability during the double-support phase of flat terrain walking. Remarkably, the property of the GCoP lying inside the contacts convex hull still holds. This stability criterion has been used by several authors (Huang et al., 2001;Wieber, 2002;Stonier and Kim, 2006) to infer stability in flat terrain walking. In partic-ular,Wieber (2002)defined a motion to be realizable if and only if the GCoP lies inside the convex hull of contact points. Even though
Popovic and Herr (2005)questioned the use of the GCoP as a way to guarantee postural stability, associated criteria are at present the most adopted for planning walking trajectories. In Section 2.3.3, we further question the GCoP as a stability criterion, focusing in particular on the scope of the current paper.
2.3.2. Mutiple non-coplanar contacts
Harada et al. (2003)defined a generalized ZMP (GZMP) and a pro-jected convex hull to formulate stability conditions for a limited class of arm/leg coordination tasks.Sardain and Bessonnet (2004)
proposed a concept of virtual surface and virtual CoP-ZMP limited to the case of two non-coplanar contacts. In spite of the adopted simplification, authors themselves admit their failure in finding an associated pseudo support polygon onto which the pseudo-ZMP stays.Hyon et al. (2007)presented a framework for computing joint torques that optimally distributes forces across multiple con-tacts; conditions for the CoP to lie within the supporting convex
hull are formulated but stability conditions are not formulated with sufficient level of details.
2.3.3. Global versus local CoP
In the present paper, we formulate a whole-body postural control, which assumes stable contacts. Stability, as defined in Section 2.2.1 guarantees time invariance of motion constraints and avoids the complications of controlling hybrid systems. Necessary and suffi-cient conditions for stability of individual contacts can be obtained by resorting to the FRI of each contact. Most of previous literature on flat terrain walking postulates the GCoP to lie in the contacts convex hull as a stability criterium. This criterium is a necessary and sufficient condition for a whole-body motion to be realizable as pointed out byWieber (2002)(see, in particular, Section 3.2 of his paper). However, it is not a sufficient condition to guarantee stability of all contacts. Section “Appendix B” provides a coun-terexample in the simple case of two coplanar contacts: the GCoP is shown to lie in the contacts convex hull but individual contacts are proven to be unstable. We therefore decide in this paper to stick to local contact stability criteria since no previous global criteria guarantee the stability of all local contacts.
2.4. TASK-SPACE INVERSE DYNAMICS
We now briefly review the vast literature on prioritized task-space inverse-dynamics control and we motivate our choices in this regard. Sentis (2007) and Park (2006) have been pioneers in the control of articulated free-floating rigid bodies exploit-ing the operational-space framework (Khatib, 1987). More recent approaches have explored the idea of simplifying the system dynamic equations by performing suitable projections onto the null space of the contact forces as proposed by Righetti et al. (2011)andAghili (2005). While being computationally efficient (i.e., a total computation time below 1 ms), all these approaches share a common drawback: contact forces cannot be controlled. As a consequence, stability of the contacts cannot be guaranteed, which may lead the robot to tip over and fall.
As opposed to these analytical solutions to the control prob-lem, an alternative numerical approach proposed by de Lasa et al. (2010)is to use a Quadratic Programing solver. This allows to include inequality constraints into the problem formulation, which can model control tasks and physical constraints (e.g., joint limits, motor-torque bounds, force friction cones). Even if this technique can guarantee contact stability, solving a cascade of Qua-dratic Programs with inequality constraints can be critical from a computational standpoint.
We decided to take an in-between approach: our framework of choice [see Section 3.4 orDel Prete et al. (2014)for details] allows to control the contact forces, but with a computational complexity of the same order of inverse-dynamics-based methods. Compared to optimization-based methods, our implementation does not allow for inequality constraints. To the best of our knowl-edge, the only real-time implementation of a cascade of Quadratic Programs with inequalities has been tested with a 14-DoF robot on a fast 3.8 GHz CPU (Herzog et al., 2014). We cannot be sure that this method will be fast enough for 26 DoFs and/or a slower CPU. For this reason, while we think that using inequali-ties could be useful, we postponed it to the (near) future because
we know that it demands for an efficient and careful software implementation.
3. MATERIALS AND METHODS
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 prop-erly defined given that the resulting force and torque might not in general be orthogonal (see Sections 2.1.2 and Appendix A.2). In any case, the counterexample in Section “Appendix B.1” suggests to consider multiple local stability criteria instead of a single global one. Local contact stability has been defined in Section 2.2.1 and it has been characterized as a condition for motion constraints to be time invariant. At present, necessary and sufficient condi-tions for contact stability have been formulated only in the case of planar rigid unilateral contacts (see Section 2.2.3). This is the reason why the scope of the current paper is limited to multiple planar contacts on rigid non-coplanar surfaces. Future extensions of the present work are foreseen in the direction of characterizing contact stability in more general situations.
In the rigid and planar contact case, contact stability has been characterized by means of the contact FRI and CoP. Both quan-tities 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 incom-plete 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 3.1). This specific design choice calls for custom algorithms for contact-wrench estimation (Section 3.2.1), internal torques measurement (Section 3.2.2), and dynamic model identification (Section 3.3). Whole-body control with multiple non-coplanar
FIGURE 1 | See the video showing the control performances of the control architecture https://www.youtube.com/watch?v=jaTEbCsFp_M.
contacts is discussed in Section 3.4 and requires all the above custom components for its implementation (Section 4).
3.1. WHOLE-BODY DISTRIBUTED WRENCH AND CONTACT SENSING The platform used to perform experimental tests is the iCub humanoid robot, which is extensively described inMetta et al. (2010). One of the main features of this system is represented by the large variety of sensors, which include whole-body distributed F/T sensors, accelerometers, gyroscopes (see Figure 2), and pres-sure sensitive skin. Furthermore the robot possesses two digital cameras and two microphones. From a mechanical standpoint, iCub is 104 cm tall and has 53 degrees of freedom: 6 in the head, 16 in each arm, 3 in the torso, and 6 in each leg. All joints but the hands and head are controlled by brushless electric motors coupled with harmonic drive gears. During experimental tests, we mainly exploit two kinds of sensors: the F/T sensors and the dis-tributed sensorized skin. The F/T sensor described inFumagalli et al. (2012)is a 6-axis custom-made sensor that is mounted in both iCub’s arms between the shoulders and elbows and in both legs between the knees and hip and between the ankles and feet. This solution allows to measure internal reaction forces, which in turn can be exploited to estimate both the internal dynamics and external forces exerted on its limbs. The robot skin (Cannata et al., 2008;Maiolino et al., 2013) is a compliant distributed pres-sure sensor composed by a flexible printed circuit board (PCB) covered by a layer of three dimensionally structured elastic fabric further enveloped by a thin conductive layer. The PCB is com-posed by triangular modules of 10 taxels, which act as capacitance gages plus two temperature sensors for drift compensation. In our experiments, iCub’s upper body was wrapped with approx-imately 2000 sensors, each foot sole is covered with 250 taxels, while 1080 further sensors are at the last design and integration stage on the lower body. Each single taxel has 8 bits of resolution, and measurements can be provided as raw data or as thermal drift compensated.
3.2. INTERNAL AND EXTERNAL (CONTACT) WRENCH ESTIMATION
Fumagalli et al. (2012) proposed a theoretical framework that exploits embedded F/T sensors to estimate internal/external forces acting on floating-base kinematic trees with multiple-branches. From a theoretical point of view, the proposed framework allows to virtually relocate the available F/T sensors anywhere along the kinematic tree. The algorithm consists in performing classical recursive Newton–Euler algorithm (RNEA) steps with modified boundary conditions, determined by the contact and F/T sensor location. It can be shown that relocation relies solely on iner-tial parameters, velocities, and accelerations of the rigid links in between the real and virtual sensors [see, in particular, the exper-imental analysis conducted byRandazzo et al. (2011)]. The pro-posed algorithm consists in cutting the floating-base tree at the level of the (embedded) F/T sensors obtaining multiple subtrees as in Figure 3. Then, each subtree is an independent articulated floating-base structure governed by the Newton–Euler dynamic equations. The F/T sensor, gives a direct measurement of one specific external wrench acting on the structure (green arrows in
Figure 3). Other external wrenches (red arrows in Figures 3 and 4)
FIGURE 2 | Mechanical schemes of the humanoid robot iCub with force/torque sensors, gyroscopes, and accelerometers highlighted in green. Left : locations of the six proximal six-axis
F/T sensors (legs and arms). Center : locations of the skin
microcontrollers, which have a 3D accelerometer embedded. Right : locations of the motor microcontrollers and the commercial inertial sensor, with the latter having a 3D gyroscope and a 3D accelerometer embedded.
FIGURE 3 | The left picture shows the location of four (out of six) F/T sensors on the iCub humanoid (sensors at the feet are omitted in this picture). The right picture shows the induced iCub kinematic
tree partitioning. Each obtained subpart can be considered an
independent floating-base structure subject to an external wrench, which coincides with the one measured by the F/T sensor (green arrow). Red arrows represent possible location for the unknown external wrenches.
3.2.1. Method for estimating external wrenches
We now describe a method for the estimation of contact wrenches; more details can be found inDel Prete et al. (2012). Let us consider a kinematic chain composed by N links, having a F/T sensor at the
base (see Figure 4), where wiis the wrench (i.e., force and moment) exerted from link i to link i + 1, ¨pciis the acceleration of the center
of mass of link i and miis the mass of link i. We know w0(i.e., the
F/ T sensor ::::: m1 m2 mN
w
0w
1w
2w
N¡1w
e1w
e2w
e3Ä
p
c 1Ä
p
c2Ä
p
cNw
eNcFIGURE 4 | Sketch of a kinematic chain with an embedded F/T sensor.
Although the sketch refers to a serial chain, the theoretical framework holds also in the case of multiple-branches articulated chains (see, for example, the torso sub-chain inFigure 3).
(i.e., the locations where the skin senses contacts), and we want to estimate the K contact wrenches we1,. . . , weK. Writing Newton’s and Euler’s equations for each rigid link and summing up all the
N resulting equations we obtain:
f0+ K X i=1 fei = N X i=1 mip¨ci, (1) µ0+ K X i=1 (µei+r0,ei×fei) = N X i=1 (r0,ci ×mip¨ci +I i iωi˙ +ωi×Iiiωi), (2)
where Iiiis the inertia of link i,ωiand ˙ωi are the angular veloc-ity and acceleration of link i, respectively, and r0,ci is the vector
connecting the chain base to the center of mass of link i. Noting that in equations (1) and (2) the only unknowns are the contact wrenches, the estimation problem may be solved rewriting these equations in matrix form Ax = b, where x ∈ Rucontains all the u contact unknowns, whereas A ∈ R6×uand b ∈ R6are completely determined. The equations are constructed taking into account the type of possible contacts among the following three: pure wrench (we, 6-dimensional vector corresponding to force and torque); pure force (fe, 3-dimensional vector corresponding to a pure force and no torque); force norm (kfek, one-dimensional unknown assuming the force to be orthogonal to the contact surface). In the simplest case, only a single contact acts on the sub-chain and the associated pure wrench can be uniquely determined (system of six equations and six unknowns). In other cases, a solution can be obtained with the following least squares procedure. The matrix
A is built by attaching columns for each contact according to its
type. The columns associated to pure wrenches (Aw), pure forces (Af), and force norm (An) are the following:
Aw = I 0 S(r0,en) I , Af = I S(r0,en) , An= ˆ un r0,en× ˆun .
where S(v) ∈ R3×3 is the skew-symmetric matrix such that
S(v)z = v × z, with × denoting the cross product operator, and
ˆ
un is the versor of the contact force fen. The matrix A mainly depends on the skin spatial calibration, which can be obtained and
refined with the procedure described byDel Prete et al. (2011). The 6-dimensional vector b is defined as:
b = fbµb = N P i=1 mip¨ci −f0 N P i=1 (r0,ci×mip¨ci +I i
iωi˙ +ωi×Iiiωi) − µ0
The vector b depends on kinematic quantities, which can be derived for the whole-body distributed gyros, accelerometers, and encoders. Details on how to estimate these quantities have been detailed byFumagalli et al. (2012). Once A and b have been com-puted, we can use the equation Ax = b for estimating external wrenches. The equation defines a unique solution if there is exactly one unknown wrench on the considered sub-chain. In the case of interest for the present paper, an exact characterization of the inter-action wrenches can be obtained if there exists exactly one contact wrench per each of the sub-chains obtained by the body structure partition induced by the F/T sensor positions (see Figure 3). In all other situations, an exact estimate cannot be obtained but from a procedural point of view it is preferable to give a reasonable esti-mate of all the contact wrenches. The solution we adopted consists in computing the minimum norm x∗
that minimizes the square error residual:
x∗=A†b
where A†is the Moore–Penrose pseudo-inverse of A. The method has been implemented as an extension of the iDyn library3and it has been integrated with other software modules to create an efficient software system able to estimate internal and external wrenches of the whole iCub robot.
3.2.2. Method for estimating internal torques
Once an estimate of external forces is obtained with the method described in Section 3.2.1, internal wrenches can also be estimated with a standard Newton–Euler force propagation recursion. Pro-jection of the internal wrenches on the joint axes provides an estimate of the joint torques τ. A torque controller with joint friction compensation guarantees that each motor provides the desired amount of torque to the joints. In order to improve the torque tracking performance, a suitable identification procedure was adopted to estimate the voltage to torque transfer function for each motor. Details are given in the following section together with some details on the dynamic model identification.
3.3. DYNAMIC MODEL ESTIMATION
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 deci-sive for the efficacy of the control algorithm that will be described in Section 3.4. A key element to improve the estimation and control
3See the software library documentation http://wiki.icub.org/iCub_documentation/
accuracy is the availability of a reliable dynamic model [masses, inertias, and center of mass positions in equations (1) and (2)]. Standard identification procedures do not apply directly and a customization to iCub specific sensor modalities and distribu-tions is necessary. In the following two subsecdistribu-tions, we describe the solution that we implemented in order to improve dynamic model accuracy (Section 3.3.1) and torque tracking performances (Section 3.3.2).
3.3.1. Dynamic model identification
The accuracy of the system dynamics equation (7) is crucial in the proposed control framework since it affects the controller equa-tion (8) and the internal/external torque estimaequa-tion procedure described in Section 3.2. Individual dynamic parameters (mass, inertia, and center of mass position) of the rigid bodies consti-tuting an articulated chain can be directly obtained from CAD drawings. These parameters are often not sufficiently accurate and standard identification procedures such as those proposed in the handbook of robotics byHollerbach et al. (2008)can be applied in order to improve modeling accuracy. Remarkably, these procedures do not give an estimate of individual parameters but some linear combination of them, known in literature as the base
parameters. It remains therefore to be clarified if the base
parame-ters suffice to implement the procedure described in Section 3.2.1 to estimate external wrenches. This procedure, written as it is, requires the knowledge of individual dynamic parameters as evi-dent from equations (1) and (2). Additionally, it needs to be verified that also the procedure for estimating internal torques presented in Section 3.2.2 can be reformulated in terms of the base parameters only. Interestingly, it can be shown, resorting to the work byAyusawa et al. (2014)that the base parameters are a subset of those used for both the estimation procedures in Section 3.2.
3.3.2. Modeling and representation of the motor transfer function Another important component in implementing the control strat-egy detailed in Section 3.4 is torque control. For the scope of the present paper, it was necessary to implement a model-based con-troller tuned for each motor. The model assumes that the i-th joint’s torqueτi is proportional (kt) to the voltage Viapplied to the associated motor, with the additional contribution of some viscous (kv) and Coulomb (kc) friction:
Vi =ktτi+(kvps( ˙θi) + kvns(− ˙θi)) ˙θi+(kcps( ˙θi) +kcns(− ˙θi))sign( ˙θi), (3) where ˙θi is the motor velocity, s(x) is the step function (1 for
x> 0, 0 otherwise) and sign(x) is the sign function (1 for x > 0,
−1 for x< 0, 0 for x = 0). Operationally, it was observed quite relevant to distinguish between positive and negative rotations as represented in the model above. We identified the coefficients
kt, kvp, kvn, kcp, kcn for each joint with an automatic procedure implemented in an open-source software module4. The motor
4https://github.com/robotology/codyco-modules/tree/master/src/modules/
motorFrictionIdentification
controller exploits the transmission model and implements the following control strategy:
Vi =kt τd i −kpτi˜ −ki Z ˜ τidt + [kvps( ˙θi)
+kvns(− ˙θi)] ˙θi+ [kcps( ˙θi) + kcns(− ˙θi)] tanh(ksθi), (4)˙ where tanh(ksθi˙) is used to smooth out the sign function, ks is a user-specified parameter that regulates the smoothing action, and
˜
τi = τi−τid is the i-th torque tracking error, and kp, ki> 0 are the low-level control gains. The control objective for the torque controller consists in obtainingτ ' τd and therefore the con-troller will make the assumption that the commanded valueτidis perfectly tracked by the torque controller. Also, observe that there is no derivative term in the parenthesis on the right hand side of equation (4). In fact, the measurement of ˙˜τiis noisy and unreliable at the current state of the iCub’s measuring devices.
3.3.3. Differential joint torque and motion coupling
Specific care was posed in controlling the torques at joints actu-ated with a differential mechanism. As an example, we consider here the torso roll, pitch, and yaw joint represented in Figure 5. In particular, define q ∈ R3 the vector of the joint angles cor-responding to the torso yaw, the torso roll, and torso pitch degree of freedom. Also, by abusing notation, define θ ∈ R3 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 = T ˙θ, T =: r R 2Rr 2Rr 0 0.5 0.5 0 −0.5 0.5 , (5)
where r and R are the radius of the pulleys sketched in Figure 5. The above matrix T is obtained by combining a classical dif-ferential coupling between pitch and yaw (first two rows) with a more complicated coupling with the roll motion (third row, see Figure 6). Defining τq to be the link torques and τθ the motor torques, the coupling induced on torques can be eas-ily obtained by imposing the equality between link and motor powers: ˙ q>τq = ˙θ>τθ ∀ ˙q, ˙θ ⇒ τq =T−>τθ with T−>:= R r 0 0 −1 1 1 0 −1 1 . (6)
In the case of coupled joints, the transformation matrix T is used in both equations (3) and (4), which hold at the motor level. Since position (˙q) and torque (τ) feedback is available at the joint level, suitable transformations need to be applied. In the case of coupled joints, our current software5implements joint per joint
5Both the identification equation (3) and control equation (4) are available with an
open source license. See the documentation in http://wiki.icub.org/wiki/CoDyCo_ Software
FIGURE 5 | The sketch represents the differential joint of the iCub torso. The pitch, yaw, and roll joints are actuated with three motors 0B4M0, 0B3M0, and
0B3M1 in differential configuration. Motor (θ ∈ R3) and joint (q ∈ R3) positions are coupled as described inFigure 6.
FIGURE 6 | The sketch is used to represent the kinematic coupling between the yaw and roll movements. A roll movement of an angle q2implies 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 q1occurs.
equations (3) and (4) with substitutionsτ ↔ τθand ˙q ↔ ˙θ where motor velocity ( ˙θ) and motor torques τθare obtained from joint velocity (˙q) and joint torques (τq) as follows:
˙
θ = T−1q,˙ τ
θ=T>τq.
3.4. COMPLETE 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 described by the following constrained differential equations: " Mb Mbj M> bj Mj # | {z } M(q) " ˙ vb ¨ qj # | {z } ˙ v +"hb hj # | {z } h(q,v) − " Jcb> J> cj # | {z } Jc(q)> f ="06×n In×n # | {z } S> τ (7a) Jc(q)˙v + ˙Jc(q, v)v = 0, (7b) where q ∈ SE(3) × Rn, with SE(3) the special Euclidian group, represents the configuration of the floating-base system, which is given by the pose of a base-frame [belonging to SE(3)] and n generalized coordinates (qj) characterizing the joint angles. Then,
v ∈ Rn+6 represents the robot velocity (it includes both ˙qj∈ Rn and the floating-base linear and angular velocity vb∈ R6), ˙v is
the derivative of v, the control input τ ∈ Rn is the vector of joint torques, M ∈ R(n+6)×(n+6) is the mass matrix, h ∈ Rn+6 contains both gravitational and Coriolis terms, S ∈ Rn×(n+6) is the matrix selecting the actuated degrees of freedom, f ∈ Rk is the vector obtained by stacking all contact wrenches, which implies that k = 6Nc and Nc the number of (rigid) contacts,
Jc∈ Rk×(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 (2013) in the context of floating-base robots. The framework computes the joint torques to match as close as possible a desired vector of forces at the contacts [equation (8a)] while being compatible with the system dynamics [equation (8b)] and contact constraints [equation (8c)]: τ∗ =arg min τ∈Rn kf − f ∗ k2 (8a) s.t . M ˙v + h − Jc>f = S >τ (8b) Jcv + ˙J˙ cv = 0 (8c) where f * ∈ Rkis the desired value for the contact forces. Then we can exploit the null space of the force task to perform N − 1 motion tasks at lower priorities. These tasks (indexed with i = 1,. . ., N − 1) are all represented as the problem of tracking a given reference
acceleration ˙vi∗for a variable xidifferentially linked to q by the Jacobian Jias follows:
˙
xi=Jiv, x¨i=Jiv + ˙J˙ iv. (9) Assuming that the force task has maximum priority the solution is: τ∗ = −(JcS¯)>f∗+Nj−1v˙1∗+ ¯S>n, (10) where Nj−1 = Mj −MbjMj−1M > bj, ¯S = h −M> bjM −1 b I i> and the term ˙v∗
1 is computed solving the following recursion for
i = N,. . ., 1:
˙
vi= ˙vi+1+(JiSN¯ p(i))
†
(¨x∗
i − ˙Jiv + Ji(U>Mb−1(hb−Jcb>f) − ¯S ˙vi+1))
Np(i)=Np(i+1)−(Ji+1SN¯ p(i+1))†Ji+1SN¯ p(i+1), (11) where U ∈ R6×(n+6)is the matrix selecting the floating-base
vari-ables, and the algorithm is initialized setting ˙vN +1=0, Np(N)=I,
JN=Jcand ¨xN=0. The implementation of this controller exploits the fact that we can compute equation (10) with an efficient hybrid-dynamics algorithm.
4. RESULTS
4.1. SET OF ADMISSIBLE TASKS
The final validation of the proposed control framework requires the definition of a suitable set of position ( ¨x∗
i) and wrences (w*) tasks and their relative priority. The set of admissible tasks is quite flexible also considering the flexibility of the underlying software libraries. Nevertheless, we list here a set of possible tasks, which we will use as a reference in the following sections. Quantities are defined with a notation similar to the one used byFeatherstone (2008): H denotes the total spatial momentum of an articulated rigid body (including linear and angular), w indicates a wrench (a single vector for forces and torques), the index i = 0, 1,. . ., NB−1 is used to reference the NBrigid bodies representing the iCub body chain (0 being defined as the pelvis rigid link), the index W is used to represent the world reference frame, the superscripts and sub-scripts la, ra, lf, and rf indicate reference frames rigidly attached to the left arm, right arm, left foot, and right foot, respectively, the superscript i indicates the reference frame attached to the i-th rigid body,kX
irepresents the rigid motion vector transformation from the reference frame i to the reference frame j,jX∗
i represents the force vector transformation from the reference frame i to the reference frame j, qjrepresents the angular position of iCub joints. Tasks will be thrown out of the following set of admissible tasks. For each task Ti, we specify the reference values (¨xi or w∗
) and associated Jacobians (Ji).
• Twrf: right foot wrench task. Regulate the right foot interaction wrench to a predefined value:
wi∗: wrf =wrf∗;
Ji: Jrf;
• Twlf: left foot wrench task. Regulates the left foot interaction wrench to a predefined value:
wi∗: wlf =wlf∗;
Ji: Jlf; • Tra
w: right arm wrench task. Regulate the right arm interaction wrench to a predefined value:
wi∗: wra=wra∗;
Ji : Jra;
• Twla: left arm wrench task. Regulates the left arm interaction wrench to a predefined value:
wi∗: wla=wla∗;
Ji : Jla;
• Tq: postural task. Maintains the robot joints qjclose to certain reference posture q∗ j: x∗ q : q¨j= ¨q∗j; Ji: I . 4.2. SEQUENCING OF TASKS
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
S1,. . ., S5each characterized by a different set of active tasks S1,
. . ., S5.
• S1has the following set of active tasks S1={Tq}.
• S2has the following set of active tasks S2=
n
Twlf, Twrf o
∪S1.
• S3has the following set of active tasks S3=Twra ∪ S2.
• S4has the following set of active tasks S4=Twla ∪ S2.
• S5has the following set of active tasks S5=Twla, Twra ∪ S2.
Transition between states is regulated by the following finite state machine where the sets Craand Clacontain the taxels (tactile elements) activated at time t.
S1 start S2 S3 S4 S5 t0∙ t ∙ t1 t > t1 Cra= ; Cla= ; Cra =;/ C la /= ; Cra =;/ Cla= ; C la /= ; Cra= ; Cr a = ; Cla /= ; Cra =;/ Cla = ; C r a /= ; C la /= ; Cla = ; Cra= ;
In practice, at start (t = t0) the robot is in state S1in order to
configuration with a postural task (Tq) that guarantees that the system is not drifting. After a predefined amount of time (t = t1), the system switches to state S2 by adding two tasks to the set of
active tasks: a control of the forces exchanged by the left and right foot (Twrf, Twlf, respectively). The control of these forces allows for a direct control over the rate of change of the momentum as will be explained in Section 4.3. Successive transitions are triggered by the tactile sensors. If no contact is detected on the right and left arm (Cra= ∅and Cra= ∅, respectively) the system remains in S2.
A transition from S2to S3is performed when the system detects
a contact on the right arm (Cra6= ∅): in this state, the active tasks are the same active in S2with the addition of the task responsible
for controlling the force at the right arm contact location (Twra). Similarly, a transition from S2to S4is performed when the system
detects a contact on the left arm (Cla6= ∅): in this state, the active tasks are the same active in S2with the addition of a task
responsi-ble for controlling the force at the left arm contact location (Twla). Finally, a transition from either S3or S4to S5is performed
when-ever the robot perceives a contact so that in this new situation both arms are in contact (Cra6= ∅and Cla6= ∅, respectively). In S5, both
arms are used to control the interaction forces by activating the tasks Twraand Twla.
4.3. TASK REFERENCES
In this section, we discuss how to compute the task references:
wra∗, w ∗ la, w ∗ rf, w ∗ lf, q¨ ∗ j.
to be used in the controller equation (8). Instantaneous values for forces are computed so as to follow a desired trajectory of the cen-ter of mass (xcomd ) and to reduce the system’s angular momentum. Instantaneous values for ¨q∗
j are chosen so as to follow a desired reference posture qdj. The latter is obtained by choosing:
¨ q∗ j(t) = ¨qjd(t) − K q d ˙ qj− ˙qjd(t) −Kpq qj−qjd(t) , (12) where Kpqand K q
d are arbitrary positive-definite matrices that take into account that in the presence of modeling errors, the acceler-ation imposed on the system ¨qj might differ from the ideal one ¨
q∗
j. Instantaneous values for interaction forces are instead com-puted to follow a prescribed center-of-mass trajectory (xcomd ) and to reduce angular momentum. In order to do so, a reference value
˙
H∗
comfor the total rate of change of spatial momentum (expressed at the center of mass) is computed with a strategy similar to equation (12). ˙ Hcom(t) = ˙H∗ comd (t) − Kdh Hcom−Hcomd (t) −K com p xcom−xcomd (t) 03×1 , Hcomd =m ˙x d com 03×1 (13) where Kdh, Kpcom are suitably defined gain matrices, Hcom is the spatial momentum around the center of mass, xcomis the center-of-mass Cartesian position, xcomd its desired value and m is the
total mass of the robot. Finally, values for fra∗, f
∗ la, f ∗ rf, f ∗ lf can be computed from ˙H∗
comconsidering that the time derivative of the momentum equals the resultant of forces and torques if all quanti-ties are computed with respect to the center of mass. The notation is slightly complicated due to the fact that in the different sce-nario states S1,. . ., S5the meaning of f (and consequently f∗) in
equation (8) changes. In particular we have:
S2: f =wrf wlf , S3: f = wra wrf wlf , S4: f = wla wrf wlf , S5: f = wra wla wrf wlf , (14)
In the different states, the following equations on f and ˙Hcom always hold:
Si: CSif + fg = ˙Hcom, (15)
where fg is the gravitational force and where we defined CSi to
be the matrix that expresses the spatial forces with respect to the center of mass: CS2 = h comX∗ rf comX ∗ lf i , CS3 = h comX∗ ra comXrf∗ comX ∗ lf i , CS4 = h comX∗ la comX ∗ rf comX ∗ lf i , CS5 = h comX∗ ra comXla∗ comX∗ rf comX∗ lf i .
In literature, these equations (derived from the Newton–Euler equations) have been presented in detail by Orin et al. (2013)
under the name of centroidal dynamics. The constraints (15) on f given ˙Hcomare not sufficient to identify a unique solution. Addi-tional constraints or requirements need to be imposed in order to properly define f∗
to achieve the desired momentum derivative. In order to get rid of this ambiguity, the following problem can be solved when at state Si:
f∗=arg min f
kf − f0k2W s.t. CSif + fg = ˙H
∗
com, (16)
where k · kW denotes a norm weighted with the matrix
W = W>> 0. The solution of this optimization is given by: f∗=C†W Si ˙ Hcom∗ −fg +(I − CS†iWCSi)f0, C†W Si =W −1C> Si CSiW −1C> Si −1 . (17)
This solution gives a set of desired forces f∗
, which generate the desired momentum derivative ˙H∗
com. It is worth noting here that additional constraints should be imposed on the contact forces to guarantee contact stability. In particular, planar unilateral contacts should have an associated FRI lying in the contact support polygon (see Section 2.2.3) and forces should be maintained within
the contact friction cones. Considering that these constraints can be approximated with a set of linear inequalities, adding them into equation (16) corresponds to transforming the problem into a quadratic program, as proposed by de Lasa et al. (2010). In the present implementation, we follow a different strategy where stability constraints are enforced by solving equation (16) with a suitable choice for the reference value f0 and weight matrix W.
To ensure that reference contact forces are stable in the sense of Section 2.2.3, we express f in a reference frame whose origin coincides with the center of the maximum circle inscribed in the contact polygon. Choosing f0 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 f0to be sufficiently
far from the cone borders. Assuming contact plane normals to coincide with the z-axis, cone borders distance is maximized with null x and y components. If a good choice for the z-axis force components is available, it can be used in f0. Otherwise a viable
choice is also to choose f0=0 since in most realistic situations the
solution f∗
is dominated by the component fg, which is always non-zero. −20 −10 0 10 [N ] f rf x + f lf x f rf y + f lf y −0.05 0 0.05 0.1 0.15 time [s] [m] xcom y com 2 6 10 14
FIGURE 7 | Results of the double-support experiment on planar contacts (left and right feet). The picture shows the time behavior of forces (top) and
center of mass position (bottom) on the sagittal (blue) and transverse (green) axis. It is worth noting that forces should be proportional to center of mass
accelerations and this is visible in the plot considering that accelerations are sinusoidal in counter phase with positions. Rapid variations of the contact forces at the time t ≈ 2[s], i.e., starting time, are due to the activation of the torque control.
FIGURE 8 | Results of the double-support experiment on planar contacts (left and right feet). The left picture shows in three dimensions the feet
contacts, the feet center of pressures, the forces at the feet and at the center of mass during three instants: at two extrema of the sinusoid (red and blue) and in the middle of the sinusoid (green). Remarkably forces are maximum at
the extrema when also accelerations are maximal. The right picture shows a close-up of the feet with the trajectory of the center of pressure, an ellipse representing a Gaussian fit of the data points and three points corresponding to the position of the centers of pressure when at the two extrema of the sinusoid (red and blue) and in the middle of the sinusoid (green).
−6 −4 −2 0 2 4 6 f la [N ] −6 −4 −2 0 2 4 time [s] f ra [N ] fx f y f z f x f y f z 28 24 32 36 40 21
FIGURE 9 | Results of the double-support experiment on four non-coplanar contacts (both feet and arms). The plots represent the evolution of the
contact forces at the left (top) and right (bottom) arms when contact is established.
FIGURE 10 | Results of the double-support experiment on four non-coplanar contacts (both feet and arms). The left picture gives a
three-dimensional view of the foot center-of-pressure positions together with the arm contact forces. Forces are represented in a color
scale that goes from black (contact establishment) to blue (steady state). The right picture gives a close-up on the foot center-of-pressure positions with an ellipse that represents the Gaussian approximation of its distribution.
4.4. EXPERIMENTAL RESULTS
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 1). The desired center of mass position was moved left to right with a sinusoidal overimposed on its initial position xcom(t0) along the
robot transverse axis (n):
xcom(t) = xcomd (t0) + n · A sin(2πfrt) (18)
with fr=0.15Hz and A = 0.02m (see Figure 7). The reference posture qjd(t) was maintained at its initial configuration qjd(t0).
As previously described the desired center-of-mass acceleration was obtained by suitably choosing the forces at the contact points (in this case at the feet) as represented in Figure 8. A video of the first phase of the experiment6is available for the interested reader.
In a second phase, the iCub was maintaining its center of mass at its initial position xcomd (t) ≡ xcom(t0) and the joint
ref-erence posture qjd(t) was chosen so as to move the arms toward a table in front of the robot. The controller equation (8) was reg-ulated by the finite state machine described in Section 4.2. At the
