WO2015056178A1 - Attractor-based whole-body motion control (wbmc) system for an articulated robot - Google Patents

Abstract

A Model-Based, Whole-Body Motion-Control (WBMC) system for an articulated robot required to perform simultaneous tasks which are not prioritized a priori and which may also be competing, the system comprising is described, the system comprising : • one or more attractors (MinEff, MomJ, MomCOM, JLim, EEff ), each of which is an operationally independent closed-loop, model-based torque/ force-control module associated with a corresponding robot's controlled task, and is designed to receive a quantity computed based on one or more quantities ( q, q ) representing the state of the robot, and to output robot's joint torques (T_GC,T_ME, T_MJ, T_LMC, T_AMC> T_JL> T_EE ) which are to be applied to robot's active joints to attract the robot's current state to a robot's target state; and • a torque and/or linear force command generator designed to receive the robot's joint torques and/or linear forces from the one or more attractors and to generate a torque and/or linear force command for the robot based on the received robot's joint torques to cause the robot's current state to converge to the robot's target state.

Description

ATTRACTOR-BASED WHOLE-BODY MOTION CONTROL (WBMC) SYSTEM FOR

AN ARTICULATED ROBOT

Technical Field of the Invention

The present invention relates to an Attractor-based Whole-Body Motion Control (WBMC) System applicable to any type of articulated robot, e.g. humanoid robots, to which the following description will exemplarily refer to without losing generality, or multi-legged robots, such as quadrupeds or hexapods or fixed-base robots, as industrial manipulators .

Background Art

With growing research interest in humanoid robotics in recent years, robots have become increasingly proficient in performing many different, non-trivial tasks, such as running, jumping, climbing stairs, and manipulating objects. In most cases, however, each of these tasks is addressed individually, and this imposes a fundamental limitation on the use of humanoids in the real world. While humans may be outperformed by robots in a single task, they are vastly more capable of adapting and combining behaviors to solve different tasks. This flexibility allows humans to generalize their knowledge, and successfully perform tasks that they have never faced before. This also opens the door for simultaneous execution of multiple tasks [1] [2] .

To address these constraints, the class of whole-body control systems is a promising research direction. They represent a wide range of complex movement skills in form of low-dimensional task descriptors which are projected on the robot's actuators, such exploiting the full capabilities of the entire body. The literature in this area is vast, and two main directions have emerged. Inverse kinematics concepts utilize kinematic redundancy resolution [3] [4] [5], and are particularly popular due to their compatibility with velocity-controlled robots. More recently, torque-controlled humanoid robots have become available. Control concepts based on various inverse dynamics formulations allow to implement whole-body controllers utilizing the dynamic equations of motion. A prominent example is the operational space control [6], that was extended to control the whole-body of humanoid robots [7] [8] . An excellent survey on such methods is given in [9] . The authors of [10] [11] [12] have further extended this formulation proposing improved contact models. A comprehensive comparative analysis of the solutions adopted can be found in [13] .

More recently, concepts from the computer graphics domain have emerged. Other than in inverse dynamic formulations, these methods represent unilateral constraints such as the contact and friction properties explicitly, and make use of quadratic programming techniques to solve for the joint torques. While they have been applied to problems in the computer graphics domain [14] [15], the high computational demands make it still hard to apply them to real-time control systems.

DIETRICH A ET AL : "Dynamic whole-body mobile manipulation with a torque controlled humanoid robot via impedance control laws", INTELLIGENT ROBOTS AND SYSTEMS (IROS), 2011 IEEE/RSJ INTERNATIONAL CONFERENCE ON, IEEE, 25 September 2011 (2011-09-25), pages 3199-3206, XP032200642, DOI: 10.1109/IROS.2011.6094445, ISBM 978-1-61284-454-1, discloses a mobile humanoid robot comprising an upper body mounted on a mobile platform, and controlled by a dynamic whole-body motion, impedance-based controller wherein competing tasks are prioritized and where low priority tasks are projected on to the null space of high priority tasks (as shown in Figure 3 on page 4) : a central section is dedicated to the high priority task, i.e., the Cartesian Impedance Control; on the left side of the figure are the low priority tasks, such as strong and weak collision avoidance, in particular upper body self-collision avoidance, platform collision avoidance, static singularity avoidance for the arms, and dynamic singularity avoidance for the platform, mechanical end stops, desired impedance for torso and mobile base, null space damping.

JOSEPH SALINE ET AL : "Synthesis of complex humanoid whole-body behavior: A focus on sequencing and tasks transitions", ROBOTICS AND AUTOMATION (ICRA), 2011 IEEE INTERNATIONAL CONFERENCE, IEEE, 9 MAY 2011 (2011-05-09) , pages 1283-1290, XP032034023, DOI : 10.1109/ICRA.2011.5980202, ISBM 978-1-61284-386-5, discloses a mobile humanoid robot required to perform a sequence of dynamic tasks ("defined as the servoing of a specific 3D frame attached to the robot body to a desired goal value", Section II.A at page 2), the simultaneous achievement of which cannot be assured and hence optimization and a strategy based on weights to represent their importance is adopted. The robots interacts with a changing environment, and the input torques are different depending on whether the robots performs tasks in a constrained state (e.g., in contact) or not.

Object and Summary of the Invention

The Applicant has experienced that the state-of-the-art solutions have proven to be very effective in most of the scenarios tested, but they can be too restrictive when pushed to the limit [7] . A typical example is a reaching task that has to be performed while maintaining the balance. In this case the highest priority is given to balancing. Usually, to guarantee that the robot can maintain its balance, the Zero-Moment Point (ZMP) [16] must remain at the center of the support polygon. This condition must continue to be satisfied when the hand reaches for the target. If the target is too far away the low priority task (reaching) is declared unfeasible and is not completed.

The aim of the present invention is therefore providing a solution that improves the performances of the state-of- the-art solutions without requiring any a priori prioritization of the tasks, in particular the competing (conflicting) ones.

This aim is achieved by the present invention, which relates to a control system as defined in the appended claims .

In broad outline, a novel approach to the whole-body motion-control of humanoid robots is proposed, wherein the compliant behavior of the robot is achieved by using so- called attractors, which are atomic control modules that are associated to each controlled task and affect the state of the robot driving it towards a more preferred one. Balance of the robot is guaranteed by a set of attractors that are designed to satisfy a basic definition of equilibrium coming from classical mechanics and based on the robot's effort and on the joint and center of mass (CoM) momenta. A computationally efficient gravity compensation for floating-base robots is also introduced.

Brief Description of the Drawings

For a better understanding of the present invention, preferred embodiments, which are intended purely by way of example and are not to be construed as limiting, will now be described with reference to the attached drawings, wherein :

• Figure 1 depicts a functional block diagram of an attractor-based WBMC System according to the present invention;

• Figures 2a to 2e depicts the behavior of the WBMC system applied to a 2-link fixed-base robot; and

· Figures 3a and 3b depict torque generated by a

Minimum Effort (MinEff) Attractor vs the angle in the case of a 1-link fixed-base system and in different formulations . Detailed Description of Preferred Embodiments of the Invention

The following description is provided to enable a skilled person to make and use the invention. Various modifications to the embodiments will be readily apparent to those skilled in the art, without departing from the scope of the present invention as claimed. Thus, the present invention is not intended to be limited to the embodiments shown, but is to be accorded the widest scope consistent with the principles and features disclosed herein and defined in the appended description and claims.

In broad outline, unlike the state-of-the-art solutions, where a very rigid system based on priorities (e.g. balancing) is given, a more flexible (or compliant) solution is preferred.

Biological findings suggest that humans, and more in general animals, are often not as precise as robots, but instead tend to minimize the error from some preferred conditions (the existence of convergent force fields as building blocks of complex behaviors was experimentally verified in frogs [17]) . It is uncommon to see a person maintaining a perfect balance, but the balanced condition is undoubtedly a preferred one, and as the error grows the corrective action will become more evident. Moreover, reaching the preferred condition is also usually more important than the specific trajectories that are used to reach it.

Following this philosophy, a Whole-Body Motion Control (WBMC) system based on a set of attractors is proposed herein. Each attractor is an atomic control module that is associated with a given task, which is represented by a physical or derived measure (in the terminology used in this description, a "physical" measure refers to a basic variable related to the state of the robot, i.e., joint/task position/velocity, while a "derived" measure is a computed variable, e.g., ZMP location, angular momentum, effort) , and works in parallel with other attractors generating joint torques that aim to modify the state of the robot so that the error in a target condition is minimized.

For an end-effector attractor this is similar to virtual model control [18], or impedance control [19], or operational space control [6]. Intuitively it is easy to visualize an end-effector attractor as a (possibly non- linear) spring connecting the current end-effector position with the target position. The same idea can be extended to non-physical variables, like the ZMP location, the centroidal angular momentum, or any other measured variable. A more precise definition of attractor will be provided hereinafter.

There is no strict priority imposed between the tasks, but the importance of the one with respect to the others is set by adjusting the weight of the attractors. The total torque applied to the joints is then a linear combination of the torques generated by each of the attractors, in accordance with the associated weights. As long as the robot has sufficient redundancy to guarantee that optimality is reached by all the variables that are controlled, the attractors are not competing (conflicting) . In the example described before, where the target hand position could not be reached while maintaining the ZMP location exactly at the center of the support polygon, appropriate tuning of the weights becomes fundamental, so that the ZMP is allowed to move away from its optimal location by a certain amount, without affecting balance. In this framework it is also possible to decide whether to be very conservative (attractors associated with balancing have a high weighting compared with other attractors), or be bolder (lowering the balance attractor weighting) .

Balance is crucial in the design of a whole-body motion control system. As mentioned above, the most frequently adopted criterion is the well-known Zero-Moment Point [16], although other criteria have been developed over the years [20] [21] . Each of these proved to be very effective in a wide range of scenarios, but still they are only sufficient conditions for stability, and do not describe in full how humans maintain balance.

The WBMC system presented herein aims to be as general as possible, without being constrained to any specific condition (ground conditions, number of contacts with the ground, etc.) . For this reason a more basic idea of equilibrium is considered, coming from the fundamentals of mechanics, and is adapted to fit the attractor-based structure of the system developed. As it will be explained in detail hereinafter, this idea of equilibrium mainly deals with the effort of the robot and its momenta. The attractors operating to maintain the system close to the equilibrium are always active, because must be guaranteed at all times, and together with other attractors that are also permanently (e.g., joint limits) or intermittently active (e.g., end-effector position) constitute the proposed attractor-based WBMC system that will be presented hereinafter .

In particular, the WBMC system is a model-based, torque-control framework. It can be applied to any robot and allows simultaneous execution of several non- prioritized tasks, which can be either permanent or temporary and which may also be competing (conflicting) . Permanent tasks are such as maintaining balance, while an example of temporary task would be to reach for an object with one hand. Each task is handled by an attractor that generates a torque command to modify the robot's current state toward a target one. In a more precise definition, an attractor / is a function having a vector of joint torques as output, which controls a certain quantity g that is itself a function of (q,q), i.e., the state of the system. This concept can be formulated as τ = f(g(q, (?)) Particularly, the attractor / is the gradient of g with respect to either q or q , hence / = Vg , where Vg = \— 1 , e.g., the MinEff described below, or Vg = \— 1, e.g., the MomCOM described below. An attractor converges to a preferred state, locally minimizing g . A specific non-zero desired value g can also be imposed. In this case the attractor / will be the gradient of the error between g and g, i.e., / = V(\g— g) . This formulation describes the typical nature of an attractor. Small variations were applied in order to adapt this concept to best fit to the specific task, e.g., see MinEff and JLim described below.

A block diagram of the proposed attractor-based WBMC system is depicted in Figure 1. The equilibrium is guaranteed by a set of four attractors labeled MinEff, MomJ, AMomCOM and LMomCOM. A repulsion from the joint limits attractor, labeled JLim, and position/ force end- effector attractors, labeled EEff, have also been developed. The modularity of the WBMC system ensures that other attractors can be easily implemented and added to the current ones (e.g., self collision avoidance) .

The overall torque applied to the joints is the linear combination of the torques generated by each attractor, plus a model-based gravity compensation that is described hereinafter : T = TQC + ^ME + ^ΤΜ] + ^TLMC + ^TAMC + ^TJL + ^ΤΕΕ + "" (1)

It is also important to notice that each quantity associated to the attractors presented has a stand-alone value, that is independent from the entire WBMC system, and can be easily adapted to be included in other kinds of control systems.

The WBMC system is based on a set of attractors that affect the robot state to minimize the error with respect to the tasks they are undertaking. They are all operationally independent, closed-loop controls, and for this reason their accuracy at steady-state is affected by constant disturbances, such as the gravitational effects.

Therefore, according to an aspect of the present invention, a model-based gravity compensation is conveniently applied. This also allows to reduce the weight of the attractors.

It has turned out that the orthogonal decomposition method for gravity compensation of floating-base system [10] [24] is advantageous due to its computational efficiency, and its open-loop character. In this approach contacts are considered as rigid, ideal constraints. Other works [14] [15] propose more accurate models that also account for unilateral contacts with friction, however they are computationally still too expensive for hard real-time demands .

The proposed invention was tested in simulation and on a torque-controlled compliant huMANoid (COMAN) robot. The robot was modeled as a 29-dofs system, 6 of which are unactuated and represent the floating base, while the other 23 are the actual joints of the robot. From the fundamental equation of the dynamics of a floating-base robot:

Af \ q q 4-(·'(g , q "™ J( ! — S^'* T neglecting the Coriolis term, and considering a static case, the following expression for gravity compensation can be derived [10] [24] :

where N_c(q) = I - J_c(q)⁺J_c(q) is the null-space of the contacts with the environment, J_c(q) = [Jci(q)^T Jc2(q)^T ... Jcm(q)^T]^T is the concatenation of the Jacobians of the m contacts, S = [0_ηχ6Ι_ηχη ] is a selection matrix, with n being the number of active dofs, I is the identity matrix, and ⁺ is the pseudo-inverse operator.

P(q) = (Nc (q) S^T) ⁺Nc (q) hence projects the 6 + n torques given by h(q) into n torques to be applied by the active joints to compensate for gravity. Whenever the system is subject to other external forces (e.g., the robot is holding an object, or is being pushed), these can be compensated at the same time adding J_ext(q)^TF_ext up to h(q), and applying the same projection P.

In order to improve the computational efficiency, P can be reduced to a simpler form. Expanding the above pseudo- inverse, we get:

Matrix N_c is idempotent: N_C ^T N_c = N_c, and we get:

P = (SN_c ^r)-ⁱSN_c If Vc is the orthonormal basis of the contact null- space, this expression can be further reduced by substituting N_c with V_CV_C ^T [25], becoming:

p = (SV_<:V S^T)^÷SV_<:V^:

In the case of n = 23 joints, as it is for the COMAN robot, with both feet on the ground, N_CS^T in (2) is a (29x23) matrix, while SV_C in (3) is a (17x23) matrix. Since the pseudo-inverse is the most expensive operation among those used, Equation (3) is used to derive P, to reduce the computational time.

Coming back to the set of attractors, balance plays a fundamental role in whole-body control. A system that aims to coordinate the simultaneous execution of many tasks is always subject to the necessary condition that the robot must not fall while performing the desired actions. As already mentioned, ZMP [16] has been the most widely adopted method to guarantee balance over the last decades. This, and other methods [20] [21] that were proposed more recently, have successfully accomplished this goal, but still represent only a sufficient condition for balancing, and do not capture the full complexity of human balance.

The proposed system aims to be as general as possible, and to not be constrained by any assumption, such as the number of contacts or the conditions of the terrain. For this reason a very basic definition of equilibrium coming from Classical Mechanics [26], page 113, was considered: "A system of particles is in static equilibrium when all the particles of the system are at rest and the total force on each particle is permanently zero". This is a very strict definition, but describes clearly what a preferred condition for a system is (i.e., an attractive situation) . In the case of floating-base multi-body systems the equilibrium refers both to the "interbodies" condition, and to the state of the system with respect to the inertial frame. The latter is well described by the CoM of the entire system. The safest and most "under control" situation for a robot can be defined by the following terms:

1) the resultant force/torque acting on the system is zero, i.e., F_C0M = 0_6X1 , where F_C0M is the vector of the spacial resultant force projected onto the CoM;

2) the "internal" torques generated by the external forces applied to the system are all zero, i.e., TJ = 0_NX1 , where Tj is the vector of the joint torques;

3) the system is at rest with respect to the world, i.e.,

^VCOM ⁼ ®6xi I I where v_C0M is the vector of the spacial velocities of the CoM;

4) the system is "internally" at rest, i.e., Vj = 0_nX1 , where Vj is the vector of the n joint velocities.

If these conditions were continuously satisfied the robot would be practically unable to act. The execution of a task requires these rules to be compliant: the robot is allowed to leave this preferred state, but never to get too far away from it. Loss of equilibrium means losing control of the robot: intuitively, if the controlled variables (velocities and forces) grow too large, the motors may not be powerful enough to generate the torques required to bring the system back to the equilibrium.

Based on these considerations, a set of attractors has been developed to guarantee that the robot always stays in the neighborhood of the equilibrium.

Figure 2 depicts how the behavior of the WBMC system is not always easy to predict. The torques generated by the MinEff attractor in the case of a 2-link fixed-base robot, for instance, aim to bring the robot to a vertical position when gravity is the only external force acting on the robot (a) . If another external force is applied (b) , instead, the MinEff locally searches for a configuration that minimizes all external disturbances. In (c) the effect of the MomJ is shown. A positive torque in both joints is generated to compensate the effects of a negative angular velocity wl in joint 1. Similarly, the AMomCOM and LMomCOM generate a torque in all joints to reduce the velocity of the CoM caused, in this case, by w₂ and v₂, respectively.

1 ) Minimum Effort (MinEff) Attractor

Conditions 1 and 2 from the list above are closely related to gravity (plus external forces) compensation. Whenever they are satisfied, h(q) + J_ext(c[ P_ext (contact reaction forces here are considered as external forces) will be equal to zero. This means that, if the system is also at rest (conditions 3 and 4), no torque needs to be applied by the motors to prevent it from starting to move (both internally and with respect to the world frame) . This also means that, as the external disturbances grow, a larger torque will be required to compensate for their effect. The robot effort may be defined as:

E = T_G ^T _CWT_GC (4) with T_GC compensating for both gravity and any other external force, and where W is a weighting diagonal matrix that can be used to favor specific joints over the others. A minimization of the effort E is therefore beneficial not only, as is obvious, to reduce the energy consumption, but also to constrain the robot to a configuration that is closer to the equilibrium. To achieve this task, an attractor to the minimum effort (MinEff) was developed. Its expected behavior is the one of a gradient descent, where the gradient of the effort can be derived as:

_{VE =}1L...1L]^T ,5,

Vdq₁ dq_n\

It can be verified that the behavior of an attractor of the kind τ_ΜΕ =—k_ME!E can be further improved.

Figure 3a depicts the torque generated by a MinEff attractor of the kind τ_ΜΕ =—k_ME!E applied to a 1-link system attached to a fixed base by a rotational joint, with only gravity upon it. As it may be appreciated, the torque generated always has a correct direction, but when the link is almost horizontal (close to π in the example) the absolute value of the torque is small. In detail, the link is forced to move to a vertical position, either up or down (+7T/2 if the zero is set at the link being horizontal) . These two configurations are both minima in terms of effort, since link and gravity are parallel. As the angle changes from +7r/2to0 (or+π), the effort E will increase proportionally to the squared cosine of the angle. The gradient descent, hence, indicates the correct direction of movement. The absolute value of VE , though, is not monotonically increasing between +π/2 and 0 (or +π) . If we consider the upper right quadrant, for instance, VE grows from zero, when the angle is π/2 , to a maximum, when the angle is π/4 , and then decreases again to zero, when the angle is 0 (the same applies to the other quadrants) . If we set T_ME oc VE , then a small torque will be generated when the links is almost horizontal, while a bigger corrective torque is expected as the robot moves away from the equilibrium.

A better formulation of the MinEff attractor, depicted in Figure 3b, is thus: τ_ΜΕ = -k_MEsign(VE) o (T_GC O T_GC) ( 6 ) where the operator o is the element-wise Hadamard product. Two discontinuities are at 0 and +π . Between these two angles (that represent the worst configurations in terms of effort) the behavior is similar to that of a quadratic spring with equilibrium at +π/2 . Tuning the weight k_ME produces more conservative or more relaxed behaviors. It is important to notice that in more complex cases (many dofs, external forces, etc) the minimum effort configuration is not known as it is in the trivial example of the 1 link system. The MinEff attractor performs a local search, and keeps modifying the configuration until a minimum is reached . 2 ) Joint Momentum (MomJ) Attractor

One of the conditions for system equilibrium is that all joint velocities are zero (condition 4 in the above list) . Intuitively, this attractor is expected to behave as a damper. There are disadvantages, however, in simply setting this attractor as τ_Μ] =—k_MJq . The first is that each joint connects two subsystems that have an inertia that is typically different from the inertia seen by other joints. A unique k_Mj would not fit all cases. A specific k would have to be associated with each joint, and consequently this leads to a difficult tuning problem. The second problem to be considered is that in a multi-dofs system each joint is affected by the motion of the other joints to which it is connected: it would be not accurate to treat each joint as if it was independent from the others .

These considerations triggered the introduction of the concept of joint momentum, defined as: h_j = Mq (7) where M is the inertia matrix of the robot. The quantity h_j has been rarely exploited to control humanoid robots, although recently Orin et al . [27] have described its relation with other momenta.

The MomJ attractor is therefore defined as:

T_MJ = -k_MJ(Mq) (8) It depends on the joint velocities (Figure 2(c)), and attracts the system to a zero momentum state.

3 ) Linear and Angular Momenta about the Center of Mass (MomCOM) Attractors

The MinEff and MomJ attractors control the quantities described in conditions 1, 2, and 4 in the above list. Whenever the system is in rigid, ideal contact with the environment, if the joint velocities are zero, the COM velocities are also zero (condition 3) . Even not considering the case of no contacts (though not so unusual, e.g., flight phase in running or jumping) having a direct control on the COM velocities is fundamental for balance. There are situations where increasing the momentum at a certain specific joint can even be desirable, for instance to compensate for a large momentum in another joint, and reduce the overall momentum of the system (i.e., the centroidal momentum) . In the formulation proposed in [27], the linear and angular momenta are treated as a unique quantity (the centroidal momentum, a fundamental physical quantity for the control of a humanoid robot [4] [28]) . In the WBMC system, instead, it was decided to keep them separated with a dedicated attractor: this allows a priority to be imposed between the two (as in [29] ) . It can for instance be preferable to have a rigid control over the linear momentum, and be willing to be more relaxed on the angular momentum, or vice versa. The desired behavior can be accomplished by properly tuning the weight of the attractors .

The linear and the angular momenta about the CoM are calculated, respectively, as follows:

(9)

where rrti is the mass of the i-th link, JT,COM,I is the translational part of the Jacobian to the CoM of the i-th link, f_{C0M i} is the skew-symmetric form of r_{C0M i} , the relative position of the CoM of the i-th link, /j is the inertia of the i-th link, and J_Ri is the rotational part of the Jacobian to the end-effector of the i-th link. In the case where the robot is standing in place the linear momentum is expected to be close to zero (Figure 2 (e) ) , while a non-zero reference can be set when the robot is, for instance, walking. If v is the vector of the desired instantaneous COM velocity, the desired linear momentum can be indicated as hg_lin =v∑imi . The LMomCOM attractor can therefore be defined as:

_ ^d (\ ^hg,lin-^hg,lin \) , ,

^TLMC,i — ^KLMC ^{1 1} I

The preferred angular momentum, instead, is typically always zero (Figure 2(d)) . The AMomCOM attractor, hence, can be expressed as:

^TAMC,l — ^KAMC

Whenever a non-zero angular momentum reference is desired, this can be easily set by modifying Equation (12) accordingly .

Maintaining equilibrium is not the only permanent task for a robot. It is vital to have accurate control on the motion of the robot near the joint limits to prevent the robot from damaging itself. A special attractor is designed to achieve this purpose: unlike the other attractors, this is more the case of a "repeller" from undesired configurations (i.e., joint limits) . In order not to affect the behavior of the other attractors unnecessarily, a piecewise function is used, such that Tj_L = 0 as long as the measured joint angle is not close to one of the limits. For each joint i , a positive and a negative (θ^~) threshold value are set. This functions both a safety margin 0.0 < Δ < 0.5 , and the joint range (q†— q^~) . In detail, Of = — A(q — q^~) , and, similarly, θ^~ = q^~ + A(q — q^~) . The joint range is hence divided into three parts by θ^~ and 6 : it works like a repulsive quadratic spring damper, when q^ exceeds the thresholds, and has no effect otherwise. If q < θ^~ , then :

When, instead, 9_t ≤ q_t ≤ 6

^LJL,i = 0 (14)

Last, if q_t > θ , then:

To avoid a "sticky" effect, Tj_Li in Equation (13) always has to be greater than or equal to zero, while in Equation (15) it has to be less than or equal to zero.

End-effector attractors (typical examples of an end- effector in a humanoid are hands and feet, but there is no restriction on the application of an end-effector attractor to any task point on the body of the robot (e.g., head, elbow, pelvis, etc.) .) work like in the operational space formulation [6] for underactuated systems [7] [8] . An end- effector force is generated from the error in the end- effector position and velocity (linear spring and damper) :

FEE = k_EE)s(x_EE ^~ *EE)

^~ *EE) · The joint torques are derived from F_EE through the end-effector Jacobian J_EE , and the projection P ( P(q) maps ( (q) + J_ext(q)^TP_ext) onto the active joints, r_GC ) :

This formulation can be easily modified to directly track a reference force instead of a position, for instance when a well determined force has to be applied in a contact .

The validity of the proposed approach was first tested in simulation. The 29-dofs model of COMAN was developed in Robotran [30], including the information on the full dynamics of the robot obtained from the CAD of the real prototype. The focus of the tests was on the interaction between the attractors responsible for the equilibrium, i.e., MinEff, MomJ, MomCOM, together with the gravity compensation term, and the repulsion from the JLim.

The tests showed that the proposed novel approach to the whole-body control of humanoid robots guarantees higher flexibility compared to the whole-body control systems in the literature that are based on inverse dynamics.

Moreover, the modular structure of the proposed control system easily allows extensions.

The described embodiment refers to joint torques but, especially in non humanoid robots, one or more or all joints can be prismatic joints: in these cases a linear force is applied and controlled.

The equilibrium condition described in the preferred embodiment of the invention is particularly useful in case of legged or humanoid robots and is of less or no importance for fixed base robots. Furthermore, for legged or humanoid robots, there exists further equilibrium criteria based on other parameters, e.g. the 'zero moment point', 'foot rotation indicator' and 'centroidal moment pivot' [16, 20] . In general, the equilibrium to which a legged or humanoid robot is compliant is represented by one or more conditions of one or more selected parameters to obtain a dynamic balance so that the robot does not fall.

In order to customize or adjust the control performance the user may weigh the contribution of each attractor using different gains, either constant or variable.

The foregoing description allows the following advantages of the present invention with respect to the state-of-the-art solutions to be appreciated.

Firstly, in the present invention the attractors are all operationally independent, while those in the state-of- the-art solutions are not. This allows to define not a strict, a priori hierarchy among tasks, and to adopt different criteria to minimize the error on different tasks .

In particular, the state-of-the-art solution disclosed in Dietrich et al . discloses a conservative approach where secondary, low priority tasks are projected in the nullspace of the main task, namely the Cartesian impedance control task (Figure 3) . Only non-competing tasks are kept in the same priority level.

In the present invention, instead, all tasks are kept in the same level, also those competing for the same resources (e.g., MinEff and EEff ) , and this results in a more flexible system, where the problem of the "unfeasible tasks" fails to exist.

Having competing tasks can lead to local minima. Using attractors of different order, as in the present invention, helps reducing these risks (e.g., the MinEff is quadratic, the EEff is linear) .

In the state-of-the-art solution disclosed in Salini et al . optimization is used, according to which weighting is done in an objective function defined as linear combination of the tasks, in accordance to given weights (which are hence use to weight the tasks), and then necessarily use a unique method to minimize the objective function, and the optimization returns a vector of torques. In the present invention, instead, each attractor works independently and generates a vector of torques. The weighting is then applied, and the final output is the linear combination of the torques returned by each attractor. Each task is hence treated independently and has an associated weight, and the minimization can be done with different criteria. If optimization is done, a unique optimization criterion has to be used, typically the gradient descend. In the present invention, each attractor can behave differently from the others (e.g., the MoMCOM uses the gradient, while the MinEff does not) .

Moreover, the state-of-the-art solution disclosed in Salini et al . is more restrictive on the definition of task (section II.A) and a frame is required to be attached to the robot body. In the present invention, instead, tasks on quantities that are not related to the robot body are also considered. Therefore, if for instance a task like "try to bring the angular momentum about the COM close to the values [4 6 2]" is considered, no frame can be attached to the angular momentum.

Moreover, in the present invention, the robot's dynamical balance is based on robot's effort and joint and center of mass momenta, which fails to be equivalent to pure ZMP . In fact, ZMP fails to work when the robot is in the flight phase of a jump or run, while in the dynamical balance considered in the present invention it makes no difference whether the robot has contacts or not. References

[1] Y. P. Ivanenko, G. Cappellini, N. Dominici, R. E.

Poppele, and F. Lacquaniti, "Coordination of locomotion with voluntary movements in humans" The Journal of Neuroscience, vol. 25, no. 31, pp. 7238-

7253, 2005.

[2] F. L. Moro, N. G. Tsagarakis, and D. G. Caldwell, "On the Kinematic Motion Primitives (kMPs) : Theory and Application" Frontiers in Neurorobotics , vol. 6, no. 10, pp. 1-18, 2012.

[3] Y. Nakamura, Advanced Robotics: Redundancy and Optimization. Addison-Wesley Publishing Company, 1991.

[4] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K.

Harada, K. Yokoi, and H. Hirukawa, "Resolved momentum control: humanoid motion planning based on the linear and angular momentum" in IEEE/RSJ International Conference on Intelligent Robots and Systems, (Las Vegas, NV, USA), 2003.

[5] M. Gienger, M. Toussaint, and C. Goerick, "Whole-body motion planning - building blocks for intelligent systems" in Motion Planning for Humanoid Robots (K. Harada, E. Yoshida, and K. Yokoi, eds . ) , Springer, 2010.

[6] 0. Khatib, "A unified approach for motion and force control of robot manipulators: The operational space formulation" IEEE Journal of Robotics and Automation, vol. 3, no. 1, pp. 43-53, 1987.

[7] L. Sentis, Synthesis and Control of Whole-Body Behavior in Humanoid Systems. PhD thesis, Stanford University, CA, USA, 2007.

[8] L. Sentis, "Compliant control of whole-body multi- contact behaviors in humanoid robots" in Motion Planning for Humanoid Robots (K. Harada, E. Yoshida, and K. Yokoi, eds. ) , Springer, 2010.

[9] J. Peters, M. Mistry, F. Udwadia, J. Nakanishi, and S. Schaal, "A unifying framework for robot control with redundant DOFs" Autonomous Robots, vol. 24, no. 1, pp. 1-12, 2008.

[10] L. Righetti, J. Buchli, M. Mistry, and S. Schaal, "Inverse dynamics control of floating-base robots with external constraints: A unified view" in IEEE

International Conference on Robotics and Automation, (Shanghai, China), 2011.

[11] M. Mistry and L. Righetti, "Operational space control of constrained and underactuated systems" in Robotics: Science and Systems Conference, (Los

Angeles, CA, USA), 2011.

[12] L. Saab, 0. E. Ramos, F. Keith, N. Mansard, P.

Soueres, and J.-Y. Fourquet, "Dynamic whole-body motion generation under rigid contacts and other unilateral constraints" IEEE Transactions on

Robotics, vol. 29, no. 2, pp. 346-362, 2013.

[13] A. Del Prete, Control of Contact Forces using Whole- Body Force and Tactile Sensors: Theory and Implementation on the iCub Humanoid Robot. PhD thesis, Istituto Italiano di Tecnologia, Italy, 2013.

[14] Y. Abe, M. da Silva, and J. Popovic, "Multiob ective control with frictional contacts" in ACM SIGGRAPH/Eurographics symposium on Computer animation, (San Diego, California), 2007.

[15] P. M. Wensing and D. E. Orin, "Generation of dynamic humanoid behaviors through task-space control with conic optimization" in IEEE International Conference on Robotics and Automation, (Karlsruhe, Germany) , 2013.

[16] M. Vukobratovic and B. Borovac, "Zero-moment point - thirty five years of its life" International Journal of Humanoid Robotics, vol. 1, no. 1, pp. 153-173, 2004.

[17] S. F. Giszter, F. A. Mussa-Ivaldi, and E. Bizzi, "Convergent force fields organized in the frog' s spinal cord" The Journal of Neuroscience, vol. 13, no. 2, pp. 467-491, 1993.

[18] J. Pratt, C.-M. Chew, A. Torres, P. Dilworth, and G.

Pratt, "Virtual Model Control: An Intuitive Approach for Bipedal Locomotion" The International Journal of Robotics Research, vol. 20, no. 2, pp. 129- 143, 2001.

[19] N. Hogan, "Impedance control: An approach to manipulation: Part I - theory" Journal of Dynamic Systems, Measurement, and Control, vol. 107, pp. 1- 24, 1985.

[20] M. B. Popovic, A. Goswami, and H. Herr, "Ground Reference Points in Legged Locomotion: Definitions,

Biological Trajectories and Control Implications" The International Journal of Robotics Research, vol. 24, no. 12, pp. 1013-1032, 2005.

[21] A. Goswami and V. Kallem, "Rate of change of angular momentum and balance maintenance of biped robots" in

IEEE International Conference on Robotics and Automation, (New Orleans, LA, USA), 2004.

[22] N. G. Tsagarakis, Z. Li, J. Saglia, and D. G.

Caldwell, "The design of the lower body of the compliant humanoid robot ccub" in IEEE International

Conference on Robotics and Automation, (Shanghai, China) , 2011.

[23] F. L. Moro, N. G. Tsagarakis, and D. G. Caldwell, "A human-like walking for the compliant huMANoid COMAN based on CoM trajectory reconstruction from kinematic Motion Primitives" in IEEE-RAS International Conference on Humanoid Robots, (Bled, Slovenia) , 2011.

[24] M. Mistry, J. Buchli, and S. Schaal, "Inverse dynamics control of floating base systems using orthogonal decomposition" in IEEE International Conference on Robotics and Automation, (Anchorage, AK, USA) , 2010.

[25] A. Escande, N. Mansard, and P.-B. Wieber, "Fast resolution of hierarchized inverse kinematics with inequality constraints" in IEEE International Conference on Robotics and Automation, (Anchorage, AK, USA) , 2010.

[26] H. C. Corben and P. Stehle, Classical Mechanics.

Courier Dover Publications, 2nd ed., 1960.

[27] D. E. Orin, A. Goswami, and S.-H. Lee, "Centroidal Dynamics of a Humanoid Robot" Autonomous Robots, 2013. (online) .

[28] B. Ugurlu and A. Kawamura, "Eulerian zmp resolution based bipedal walking: Discussions on the intrinsic angular momentum rate change about center of mass" in IEEE International Conference on Robotics and Automation, (Anchorage, AK, USA), 2010.

[29] S.-H. Lee and A. Goswami, "A momentum-based balance controller for humanoid robots on non-lever and non- stationary ground" Atonomous Robots, vol. 33, no. 4, pp. 399-414, 2012.

[30] H. Dallali, M. Mosadeghzad, G. Medrano-Cerda, N.

Docquier, P. Kormushev, N. Tsagarakis, Z. Li, and D. Caldwell, "Development of a dynamic simulator for a compliant humanoid robot based on a symbolic multibody approach" in International Conference on Mechatronics , (Vicenza, Italy), 2013.

[31] B. J. Stephens and C. G. Atkeson, "Dynamic balance force control for compliant humanoid robots" in IEEE/RSJ International Conference on Intelligent

Robots and Systems, (Taipei, Taiwan), 2010.

Claims

1. A Model-Based, Whole-Body Motion-Control (WBMC) system for an articulated robot required to perform simultaneous tasks which are not prioritized a priori and which may also be competing, the system comprising:

• one or more attractors (MinEff, MomJ, MomCOM, JLim, EEff ) , each of which is an operationally independent closed-loop, model-based torque/ force-control module associated with a corresponding robot's controlled task, and is designed to receive a quantity computed based on one or more quantities ( q , q ) representing the state of the robot, and to output joint torques and/or linear forces ( ^TGC< ^ΤΜΕ· ^ΤΜ]· ^TLMC> ^ΤΑΜ0· ^τ]ΐ· ^ΤΕΕ ) which are to be applied to robot's active joints to attract the robot's current state to a robot's target state; and

• a torque and/or linear force command generator designed to receive the joint torques and/or linear forces from the one or more attractors and to generate a torque and/or linear force command for the robot based on the received joint torques and/or linear forces to cause the robot's current state to converge to the robot's target state .

2. The system of claim 1, wherein the joint torques and/or linear forces ( x_GC, τ_ΜΕ, τ_Μ]ι r_LMC, x_AMC, x_jL, τ_ΕΕ ) are compliant to a given robot's equilibrium definition based on a robot's dynamical balance such as to guarantee that the robot stays in the neighborhood of the equilibrium.

3. The system of claim 2, wherein the robot's dynamical balance is based on robot's effort and joint and center of mass momenta.

4. The system of any one of the preceding claims, wherein the one or more attractors comprise the following attractors : • a minimum effort attractor (MinEff) designed to attract the robot's effort to a minimum effort;

• a joint momentum attractor (MomJ) design to attract current joint momentum to a zero joint momentum;

· linear and angular momenta about the center of mass attractors (MomCOM) design to attract current linear and angular momenta about the center of mass to target linear and angular momenta about the center of mass;

• a repulsion from the joint limits attractor (JLim) design to repel current joint positions from joint limit positions; and

• position/ force end-effector attractors (EEff) designed to attract current positions of or forces applied to end-effectors to target positions or forces.

5. The system of any one of the preceding claims, wherein the torque and/or linear force command generator is designed to weigh the output of the one or more attractors according to a set of corresponding gains, either constant or variable.

6. The system of any one of the preceding claims, wherein the torque and/or linear force command generator is designed to:

compute an overall torque and/or linear force to be applied to the robot's joints as a linear combination of the joint torques and/or linear forces from the one or more attractors; and

generate the torque and/or linear force command for the robot based on the computed overall torque and/or linear force.

7. The system of any one of the preceding claims, further comprising:

• an open-loop, model-based, gravity-compensation torque/ force-control module (GComp) designed to receive a quantity representing the state of the robot and to output robot's joint torques and/or linear forces to be applied to robot's active joints to compensate for gravity;

and wherein the torque and/or linear force command generator is further designed to compute the overall torque and/or linear force to be applied to the robot's joints also based on the joint torques and/or linear forces from the gravity-compensation torque/ force-control module and wherein the following reduced dimension matrix is used:

where S = [0_ηχ6Ι_ηχη] is a selection matrix, n is the number of active dofs, I is the identity matrix, ⁺ is the pseudo- inverse operator, and V_c is an orthonormal basis of the contact null-space.

8. The system of any one of the preceding claims, wherein the quantities ( q , q ) representing the state of the robot comprise robot's joint positions and velocities.