submitted to ieee transaction on robotics 1 dynamic whole...

Submitted to IEEE Transaction on Robotics 1

Dynamic Whole-Body Motion Generationunder Rigid Contacts and other Unilateral Constraints

Layale Saab, Oscar E. Ramos, Francois Keith, Nicolas Mansard, Philippe Soueres, Jean-Yves Fourquet

Abstract—The most widely-used technique to generate whole-1

body motions on a humanoid robot accounting for various tasks2

and constraints is the inverse kinematics. Based on the task-3

function approach, this class of methods makes possible the4

coordination of the robot movements to execute several tasks in5

parallel and account for the sensor feedback, in real-time thanks6

to the low computation cost. To some extent, it also enables7

dealing with some of the robot constraints (e.g. joint limits or8

visibility) and managing the quasi-static balance of the robot. In9

order to fully use the whole range of possible motions, this paper10

proposes to extend the task-function approach to handle the11

full dynamics of the robot multi-body along with any constraint12

written as equality or inequality of the state and control variables.13

The definition of multiple objectives is made possible by ordering14

them inside a strict hierarchy. Several models of contact with the15

environment can be implemented in the framework. We propose16

a reduced formulation of the multiple rigid planar contact that17

keeps a low computation cost. The efficiency of this approach is18

illustrated by presenting several multi-contact dynamic motions19

in simulation and on the real HRP-2 robot.20

Index Terms—Humanoid robotics, redundant robots, dynam-21

ics, force control, contact modeling.22

I. I NTRODUCTION23

T HE GENERATION of motion for humanoid robots is a24

challenging problem, due to the complexity of their tree-25

like structure and the instability of their bipedal posture[3].26

These robots are equipped with a large number of degrees27

of freedom (DOF), typically more than 30. In return, they28

are subject to various sets of constraints (balance, contact,29

actuator limits), that reduce the space of possible motions.30

These constraints can typically be formulated as equalities31

(e.g. zero velocity at rigid-contact points [4]), and inequalities32

(e.g. joint limits [5], obstacles [6], joint velocity and torque33

within given bounds). Moreover, they are of relative impor-34

tance (e.g. balance has to be considered more important than35

visibility [7]). In total, the motion has to be designed in a set36

that lives in the high-dimensional configuration space but is37

implicitly limited to a much smaller submanifold by the set of38

constraints. This makes the classical sampling methods such as39

probabilistic roadmap [8] or rapidly-exploring random tree [9]40

more difficult to use than for a classical manipulator: the set41

of available motion cannot be sampled directly but has to be42

L. Saab, O. Ramos, N. Mansard and P. Soueresare with LAAS-CNRS, Univ. Toulouse, France,{lsaab,oramos,nmansard,soueres}@laas.fr. F. Keithis with LIRMM, CNRS, Montpellier, France, [email protected]. Fourquet is with LGP-ENIT, Univ. Toulouse, Tarbes, [email protected]

The paper was presented in part to IEEE ICRA 2011 [1] and IEEE/RSJIROS 2011 [2].

Fig. 1. Dynamic multi-contact motion with the HRP-2 model.

reached by a dedicated projection function [10]; then, due to 43

the high dimension of the configuration space, the connection 44

process is costly [11]; and the rate of connection rejectionis 45

high, due to the number of constraints. 46

Rather than designing the motion at the whole-body level47

(configuration space), the task function approach [12], [13] 48

proposes to design the motion in a space dedicated to the49

task to be performed. It is then easier to design the reference 50

motion in the task space, and transcripting this reference from 51

the task space to the whole-body level is only a numerical52

problem. This approach is versatile, since the same task is53

generally transposable from one robot or situation to another. 54

It also eases the use of sensory feedback, since the sensory55

space is often a good task-space candidate [14], [15]. 56

A task can be seen as a basic brick of motion. To generate57

a complex motion on a humanoid robot, several tasks have58

to be combined, sequentially [16] or simultaneously. The59

simultaneous execution of two tasks on a robot can be achieved 60

in two ways: by setting respective weights between the tasks, 61

or by imposing a strict hierarchy between them. Coming62

from numerical optimization [17], this second solution was63

introduced in robotics by [18], formalized for any number of64

tasks in [19], [20] and applied in the humanoid framework65

in [21]. These approaches, based on pseudoinversion and iter- 66

ative projections in the null space of prior constraints, are well 67

fitted to cope with equality constraints. However, inequality 68

constraints cannot be taken into account explicitly. Therefore, 69

approximate solutions, such as potential field approaches [22], 70

[7] or damping functions [5], [23], have been proposed to71

consider unilateral constraints. 72

The transcription of the motion reference from the task73

space to the whole-body control is naturally written as a74

quadratic program (QP) [24]. A QP is composed of two75

layers, namely the constraint and the cost. It can be seen as76

a hierarchy of two levels, the constraint having priority over 77

the cost. If only equality constraints are considered, the QP 78

resolution corresponds to the inversion schemes [20], in the 79


particular case of two levels. Inequalities can also be taken into1

account directly, as constraints, or in the cost function [25].2

In [26], a method to extend the QP formulation to take into3

account any levels of priority is given. The solution of such4

a hierarchical problem is computed by solving a cascade of5

QP (or hierarchical QP). In [27], a dedicated solver has been6

proposed to obtain the solution in one step inside a cascade,7

which reduces the cost.8

All these works only consider the kinematics of the robot.9

On a humanoid robot, many constraints arise from the dynam-10

ics of the multi-body system. The formulation by task can be11

extended to compute the torque at the whole-body level from12

the reference motion expressed in the dedicated task space,13

also calledoperational space[28]. For a humanoid in contact,14

the motion is constrained to the submanifold of configurations15

that respects the contact model [29] as illustrated by Fig. 1. A16

review of the work in modeling and control of the dynamics17

of a set of bodies in contact is proposed in [30], [31]. The18

link with inverse dynamics has been done in [32], [33].19

Using these approaches, it is possible to take into account20

a hierarchy of tasks and constraints (orstack of tasks [34]),21

all written as equalities [35], [36]. In [37], a first solution to22

handle inequalities in the stack of tasks was proposed, but23

cannot set any inequality constraint on the contact forces.24

In [38], [39], the inverse-dynamics problem has been written25

as a QP, where the unilateral contact constraints, along with26

classical unilateral constraints (joint limits, etc) are explicitly27

considered. In that case, several tasks can be composed by28

setting relative weights, but a hierarchy of tasks is not possible.29

In this paper, we propose a generic solution to take into30

account equalities and inequalities in a strict hierarchy to31

generate a dynamic motion. This solution is based on the32

links between inverse kinematics and inverse dynamics. In33

Section II, the inverse-kinematics scheme is recalled, written34

into a general form; the possibility of taking into account35

inequalities is then introduced using the solver [26], [27].36

Then, putting the operational-space inverse dynamics under37

the same generic form, we propose in Section III to use the38

same hierarchical solver to take into account both dynamics39

and inequalities. This first solution deals with the robot infree40

space. In Section IV, contacts are introduced in the model and41

used in the resolution scheme. The contact model is generic42

and can be adapted to various situations (rigid contact, friction43

cone [40], elastic contact [41]). A solution is proposed in44

Section V to implement a reduced form of multiple plane/plane45

slidingless rigid contacts. In Section VI the link is made46

with the zero-momentum point (ZMP) contact criterion [42]47

classically used in humanoid robotics [43]. Typical examples48

are shown in Fig. 1, with the HRP-2 robot using multiple non-49

coplanar contacts to perform a dynamic motion. The motion50

generation method takes around 20ms for one step of motion51

on a typical humanoid robot (30DOF). It is thus not possible52

yet to apply it as a control scheme on the robot. However, it53

can be used to generate approximate real-time motions which54

are consistent with the robot dynamics and to replay them with55

the real robot. Some examples of complex motions involving56

non-coplanar contacts and their execution on the real robotare57

presented in Section VII.58

II. I NVERSEK INEMATICS 59

A. The task-function approach 60

The task-function approach [13], or operational-space ap-61

proach [28], [44], provides a mathematical framework to62

describe tasks in terms of specific output functions. The task 63

function is a function from the configuration space to an64

arbitrary task space, chosen to ease the observation and the65

control of the motion with respect to the task to perform. 66

A task is defined by a triplet(e, e∗, Q), wheree is the task 67

function that maps the configuration space to the task space,68

e∗ is the reference behavior expressed in the tangent space to69

the task space ate. Q is the differential mapping between the 70

task space and the control space of the robot which verifies71

the relation: 72

e + µ = Qu (1)

whereu is the control in the configuration space andµ is the 73

drift of the task. To compute a specific robot controlu∗ that 74

performs the referencee∗, any numerical inverse ofQ can be 75

used. The generic expression of the control law is then : 76

u∗ = Q#(e∗ + µ) + Pu2 (2)

In this expression, the first part performs the task, and the77

second part, modulated by the secondary control inputu2, 78

expresses the redundancy of the task [18]. In the first term,79

Q# is any reflexive generalized inverse ofQ, often chosen to 80

be the (Moore-Penrose) pseudoinverseQ+ [45] or a weighted 81

inverseQ#W [46] (see App. A). In the second term of (2), 82

P = I − Q#Q is the projector onto the null space ofQ 83

corresponding toQ#. 84

B. Hierarchy of tasks 85

The projectorP is intrinsically related to the redundancy 86

of the robot with respect to the taske. A secondary task 87

(e2, e∗2, Q2) can be executed usingu2 as a new control input. 88

Introducing (2) ine2 + µ2 = Q2u gives: 89

e2 + µ2 = Q2u2 (3)

with µ2 = µ2 − Q2Q#(e∗ + µ) and Q2 = Q2P. This last 90

equation fits the template (1), and can be solved using the91

generic expression (2) [20]: 92

u2∗ = Q2

#(e∗ + µ2) + P2u3 (4)

whereP2 enables the propagation of the redundancy to a third93

task using the inputu3. By recurrence, this generic scheme can94

be extended to any arbitrary hierarchy of tasks. 95

C. Inverse kinematics formulation 96

In the inverse-kinematics problem, the control inputu is 97

simply the robot joint velocityq. The differential link Q 98

between the task and the control is the task JacobianJ. In 99

that case, the driftµ = ∂e∂t

is often null, and (1) is written: 100

e = Jq (5)


The simplest and most-often used solution is to chooseQ# to1

be the pseudoinverseQ+, that gives the least Euclidean norm2

of both q and e∗ − Jq [47], [48]. The control law is then:3

q∗ = q∗1 + Pq2 (6)

where q∗1 = J+e∗. A typical reference behavior is an expo-4

nential decay ofe to zero: e∗ = −λe, λ > 0.5

It may happen thatJ becomes singular, i.e.rank(J) < r0,6

wherer0 is the nominal rank ofJ out of the singular config-7

uration. Numerical problems can occur during the transition8

from the nominal situation to the singular one. To avoid these9

problems, the pseudoinverse is often approximated by the10

damped least-squareJ† defined by [49], [50]:11

J† =

[JηI

]+ [I0

](7)

where I is the identity matrix of proper size andη is a12

damping factor, chosen as an additional parameter of the13

control (typically,η = 10−2 for a humanoid robot).14

D. Projected inverse kinematics15

Consider a secondary task(e2, e∗2, J2). The template (3) is16

written:17

e2 − J2q∗1 = J2Pq2 (8)

In this case, the differential link is the projected JacobianQ =18

J2P, and the drift isµ = −J2q∗1 . The control inputq∗2 is19

obtained once more by numerical inversion [20], [21]:20

q∗2 = (J2P)+(e2 − J2q

∗) + P2q3 (9)

whereP2 is the projector into the null space ofJ2P . The same21

scheme can be reproduced iteratively to take into account any22

number of tasks untilPi is null.23

In generalrank(J2P) ≤ rank(J2) ≤ r2, wherer2 is the24

nominal rank ofJ2. When the second inequality is strict, the25

singularity is said to bekinematic; when the first inequality is26

strict, the singularity is said to bealgorithmic [51]1. To avoid27

any numerical problem in the neighborhood of the singularity,28

a damped inverse can be used to invertJ2P.29

E. Hierarchical quadratic program resolution30

1) Generic formulation:When considering a single task,31

the solution obtained with the pseudoinverse (2) is known tobe32

the optimal solution of the QPminu

||Qu− e∗−µ||2. The great33

advantage of the QP formulation is that both linear equalities34

and inequalities can be considered, while the pseudoinverse-35

based schemes presented above cannot explicitly deal with36

inequalities. A quadratic program is composed of a quadratic37

cost function to be minimized while satisfying the set of38

constraints [52]. It can be seen as a two-level hierarchy, where39

the set of constraints has priority over the cost. Inequalities are40

set as the top-priority. The introduction of slack variables is a41

classical solution to handle an inequality at the second priority42

level [53]. In [26], it was proposed to use the slack variables43

1Both cases are similar in the sense that

[

J1J2

]

is singular.

to generalize the QP to more than two levels of hierarchy and44

thus to build a hierarchical quadratic problem (HQP) handling 45

inequalities. 46

The HQP formulation is first recalled in a generic frame.47

A generic constraintk is defined by the linear mapAk and 48

the two inequality bounds(bk, bk), where b

kand bk are 49

respectively the lower and upper bounds on the reference50

behavior2. At level k, the cascade algorithm solving the51

hierarchy(Ak, bk) is expressed by the following QP: 52

minuk,wk

||wk||2

s.t. bk−1

≤ Ak−1uk +w∗k−1

≤ bk−1

bk

≤ Akuk + wk ≤ bk

(10)

where Ak−1, (bk−1

,bk−1) are the constraints at all the 53

previous levels from 1 tok − 1 (Ak−1 = (A1, ..., Ak−1)), 54

andAk, (bk, bk) is the constraint at levelk. 55

The slack variable3 wk is used to add some freedom to56

the solver if no solution can be found when the constraintk is 57

introduced under thek−1 previous constraints:wk is variable 58

and can be used by the solver to relax the last constraintAk. 59

On the other hand,w∗k−1

is constant and set to the result60

of the previous optimization of thek − 1 first QP (at each 61

of the iterations of the cascade,w∗k−1

is augmented with the 62

optimalw∗k by w

∗k−1

:= (w∗k−1

, w∗k)). A solution to thestrict 63

k − 1 constraintAk−1 is then always reached, even if the64

slack constraintAk is not feasible: this corresponds to the65

definition of the hierarchy. 66

A classical method to compute the solution of a QP or HQP67

relies on an active-search algorithm [52], [27] (see App. B), 68

which implies iterative computations of the pseudoinverseof 69

a subproblem of the initial QP. Since pseudoinverses are used, 70

the classical numerical problems can occur in the neighbor-71

hood of singularities. Regularization methods that extendthe 72

damping inverse [50] used in robotics can be applied [54]. 73

The method proposed above is generic and can be applied74

to any numerical problem written with a linear hierarchical75

structure. In that case, it is referred to as HQP (or cascade of 76

QP) and denoted with the lexicographic order:(i) ≺ (ii) ≺ 77

(iii) ≺ ... which means that the constraint(i) has the highest 78

priority. In the following, we propose a solution to apply this 79

formulation to invert kinematics and dynamics. The constraints 80

are then the tasks defined above and the hierarchical solver will 81

be called a stack of tasks (SOT) or hierarchy of tasks. 82

2) Application to inverse kinematics:When considering a 83

single task, inversion (6) corresponds to the optimal solution 84

to the problem: 85

minq

||Jq − e∗||2 (11)

By applying the QP resolution scheme, both equalities and86

inequalities can be considered. Replacingb by e, the reference 87

part is then rewritten: 88

e∗ ≤ e ≤ e∗

(12)

2Specific cases can be immediately implemented:bk= bk in the case of

equalities andbk= −∞ or bk = +∞ to handle single-bounded constraints.

3w is an implicit optimization variable whose explicit computation can beavoided when formulating the problem as a cascade. It does notappear in thevector of optimization variablesu. See [27] for details.


For instance, in the case of two tasks with priority ordere1 ≺1

e2, the expression of the QP is given by:2

minq,w2

||w2||2

s.t. e∗1 ≤ J1q + w∗1 ≤ e

∗1

e∗2 ≤ J2q + w2 ≤ e∗2

(13)

In robotics, when a constraint is expressed as an inequality,3

it is very likely to be put as the top priority: typically, joint4

limits and obstacle avoidance. Using this framework, it is also5

possible to handle inequalities at the second priority level6

(i.e. in the cost function). A typical case is to prevent visual7

occlusion when possible, or to keep a low velocity if possible,8

without disturbing the robot behavior when it is not necessary.9

In the sequel, the HQP considering linear equalities and in-10

equalities will be extended from inverse kinematics to inverse11

dynamics.12

III. I NVERSEDYNAMICS13

In this section the case of a contact-free dynamical multi-14

body system without free-floating root is considered.15

A. Task-space formulation16

As previously stated, a task is defined by a task functione, a17

reference behavior and a differential mapping. At the dynamic18

level, the reference behavior is specified by the expected task19

acceleratione∗, while the control input is typically the joint20

torquesτ . The operational-space inverse dynamics then refers21

to the problem of finding the torque control inputτ that22

produces the task referencee∗, using any necessary joint23

accelerationq. The accelerationq is then a side variable,24

that does not require to be explicitly computed during the25

resolution. Contrary to the case of kinematics, the mapping26

between the control inputτ and the task space is obtained27

in two stages. First, the link between accelerations in the28

configuration space and in the task space is obtained by29

differentiating (5):30

e = Jq + Jq (14)

Then, the dynamic equation of the system expressed in the31

joint coordinates is deduced from the mechanical laws of32

motion [55].33

Aq + b = τ (15)

where A = A(q) is the generalized inertia matrix of the34

system,q is the joint acceleration,b = b(q, q) includes all the35

nonlinear effects including Coriolis, centrifugal and gravity36

forces andτ are the joint torques. The generic form (1) is37

obtained by replacingq in (15) with (14) [28]:38

e − Jq + JA−1b = JA−1τ (16)

This equation follows the template (1) withQ = JA−1, µ =39

−Jq + JA−1b andu = τ .40

The torqueτ∗ that ensurese∗ is solved using the generic41

form (2). It is generally proposed to weight the inverse by42

the inertia matrixA. This weight ensures that the process43

is consistent with Gauss’ principle [56], i.e. that the torques44

and accelerations corresponding to the redundancy of the task45

are the closest to the acceleration of the unconstrained multi- 46

body system. This principle can be intuitively understood by 47

considering the weight like a minimization of the acceleration 48

pseudo energyqTAq [57], [32]. 49

The redundancy can also be explicitly formulated during50

the inversion, using the form (3). A SOT can be iteratively51

built, with the lower-priority tasks being executed in the best 52

possible way without disturbing the higher priority tasks [58], 53

[59]: 54

τ∗ = τ∗1 + Pτ2 (17)

whereP =I−JT (JA−1JT )+JA−1 is the projector in the null 55

space ofJA−1 andτ∗1 =(JA−1)#A(e∗−Jq+JA−1b). 56

B. Projected inverse dynamics 57

As before, the differential link for the projected secondary 58

taske2 is obtained by replacing (17) into the robot dynamics59

equation in the task spacee2 − J2q + J2A−1b = J2A

−1τ : 60

e2 + µ2 = Q2τ2 (18)

with µ2 = −J2q + J2A−1b− J2A

−1τ∗1 , andQ2 = J2A−1P. 61

The same weighted inverse is used to invertQ2 [58], [59]. 62

Accordingly, any number of tasks can be added iteratively63

until the projector becomes null. 64

The same singularities as in inverse kinematics may appear65

(the dynamics in itself does not bring any new singular case,66

sinceA is always full rank). To avoid any numerical problem,67

the damped weighted inverse is generally used. As for the68

kinematics, only tasks defined by equality constraints can be 69

taken into account using this pseudoinverse-based resolution. 70

To take into account inequalities, we propose to extend to the 71

dynamics the HQP [26] that was previously introduced for the72

kinematics. 73

C. Application of the QP solver to the inverse dynamics 74

When resolving a given taske while taking into account the 75

dynamics, both (14) and (15) must be fulfilled. There are two76

ways of formulating the QP. First,q can be substituted from 77

(14) into (15), to obtain the single reduced equation (16). In 78

that case, the QP only requires to solveτ , the variableq being 79

not explicitly computed: 80

minτ

||JA−1τ − e∗ − µ||2 (19)

Alternatively, (14) can be solved under the constraint (15). 81

Using the hierarchy notation, the HQP is thus (15)≺ (14), or 82

using the standard QP notation: 83

minτ,q,w

||w||2

s.t. Aq + b = τ

e∗ + w = Jq + Jq

(20)

In that case, bothτ and q are explicitly computed. They 84

constitute the vector of optimization variablesu = (τ, q). 85

QP (19) has a reduced form, but QP (20) allows any explicit86

formulation using the dynamics variables. In the following, we 87

will show that such an exhaustive formulation is important to 88

deal with the contact. 89


IV. I NVERSE DYNAMICS UNDER CONTACT CONSTRAINTS1

A. Insertion of the contact forces2

In the previous section, the considered multi-body system3

was in free space (no contact forces) and fully actuated (no4

free-floating body, for example). The model of the humanoid5

robot includes both the contact forces and a zero-torque6

constraint on the six first DOF. First, the case of a single7

contact point denoted byxc is considered:8

Aq + b+ J⊤c f = ST τ (21)

where A and b are defined as before,q is the vector of9

generalized joint accelerations4, f is the 3D contact force10

applied at the contact pointxc, Jc = ∂xc

∂qis the Jacobian11

matrix of xc 5 andS = [0 I] is a matrix that allows to select12

the actuated joints.13

The rigid-contact condition implies that there is no motion14

of the robot contact bodyxc i.e. xc = 0, xc = 0. For a given15

state, it implies the linear equality constraint:16

Jcq = −Jcq (22)

If multiplying (21) by JcA−1 and substituting the expression17

of Jcq given by (22), a constraint is obtained, that links the18

torque to the contact force:19

JcA−1J⊤

c f = JcA−1(ST τ − b) + Jcq (23)

In this expression, the acceleration does not appear explicitly20

anymore. In the basic case,JcA−1J⊤c is invertible, andf can21

be deduced [36]:22

f = (J⊤c )A

−1#(ST τ − b) + (JcA−1J⊤

c )−1Jcq (24)

This expression off can be re-injected in (21), to obtain a23

reformulated dynamic equation where the force variable does24

not appear explicitly anymore.25

Aq + bc = PcST τ (25)

wherePc = (I − Jc#A−1

Jc)T = (I − (JcA

−1)#AJcA−1)26

is the projection operator of the contact6, and bc = Pcb +27

J⊤c (JcA

−1J⊤c )−1Jcq. As above, the differential link between28

the task and the torque input is expressed through the inter-29

mediate variableq by inserting (25) in (14):30

e + µ = Qτ (26)

with µ = −Jq + JA−1bc andQ = JA−1PcST . By inverting31

(26) and choosing a proper weighted inverse, the obtained for-32

mulation is equivalent to the operational-space inverse dynam-33

ics developed in [61] (see Appendix C). When inverting (26),34

it is possible to explicitly handle the redundancy using the35

inversion template (3). The scheme can be propagated to36

any levels of hierarchy. The general form of the inverse for37

the second level of the hierarchy isJ2P1A−1PcS

T , where38

4To be exact,q should be written

[

vfqA

]

, wherevf is the 6D velocity of the

robot root andqA the position of the actuated joints. For the ease of notationq, q and q will be used in the article.

5The coordinates ofxc, f andJc have to be expressed in the same frame,for example the one attached to the corresponding robot body

6The exact same form can be obtained ifJc is rank deficient [60].

P1 is the projector into the null space of the main task.39

In general,rank(J2P1A−1PcS

T ) ≤ rank(J2A−1PcS

T ) ≤ 40

rank(J2) ≤ r2. If the first inequality is strict, this is the 41

algorithmic singularity encountered in inverse kinematics. If42

the last inequality is strict, it is akinematic singularity. If 43

the intermediate inequality is strict, the singularity is due 44

to the dynamic configuration of the multi-body system in45

contact, and could be called adynamicsingularity7. As above, 46

a damped inverse is used in practice to avoid the numerical47

problems in the neighborhood of the singularity. 48

As before, (26) follows the template (2) and can be directly49

formulated as a QP. The QP can be expressed under a reduced50

form as proposed in [2]. Or more simply, the HQP (20) can51

be reformulated to consider the dynamics in contact. Using52

the HQP notation, the program for one task is (21)≺ (22) 53

≺ (14). The variablesf and q are then explicitly computed: 54

u = (τ, q, f). This HQP was proved to be equivalent to the55

reduced inversion in [1]. 56

B. Rigid-point-contact condition 57

For a single point in rigid contact with a surface, there are58

two complementary possibilities: either the force along the 59

normal to the contact surface is positive (the robot pushes60

against the surface and does not move), or the acceleration61

along the normal is positive (the robot contact point is taking 62

off, and does not exert any force on the surface). Both63

possibilities are said to be complementary since one and only 64

one of them is fulfilled. This is mathematically written: 65

x ≥ 0 (27)66

f⊥ ≥ 0 (28)67

xf⊥ = 0 (29)

wheref⊥ is the component off corresponding to the normal 68

direction. The complementary condition is a direct expression 69

of d’Alembert-Lagrange Virtual Work principle, in the simple 70

case of rigid contact. By writing (21) and (22), it is implicitly 71

considered that the robot is in the first case: no movement72

(22) and positive normal force. In consequence, the generated 73

control must also fulfill the second condition (28). 74

Very often, only the non-motion condition constraint (22)75

is considered [36]. As a consequence, an unfeasible dynamic76

motion can be generated since the second contact condi-77

tion (28) is not explicitly verified. A first solution can be 78

to saturate the part of the control that does not correspond79

to gravity compensation when the positivity condition is not 80

satisfied [59]. However, such a solution is very restrictive81

compared to the motions that the robot can actually perform.82

It is straightforward to take into account the two conditions 83

above in a HQP. In that case, the contact forces have to be84

explicitly computed as one of the QP variables:u = (τ, q, f). 85

The HQP is then (21)≺ (22) ≺ (28) ≺ (14). 86

7The three cases are similar in the sense that the matrix

J1 0 0J2 0 0A Jc −ST

is singular.


The two first levels (21), (22) are always feasible. However,1

it may happen that (28) is not. This case is sometimes referred2

to asstrong contact instability[62]: whatever the motions of3

the multi-body system are, the contact cannot be maintained.4

In practice, the solver will find an optimalu, but with nonzero5

slack variables corresponding to (28). The solutionu is then6

meaningless, since it is dynamically inconsistent. To obtain a7

consistent control in that case, a change of behavior should8

be triggered, with the robot removing one of its contacts9

from (22) and trying to find another solution without this10

contact. However, the nonzero slack on (28) will only appear11

in extreme cases, for example when the robot is already falling,12

and in general it is already too late to do anything to restore13

the balance.14

The typical situation with a humanoid robot requires more15

than one contact point: for example, when one rectangular foot16

is in contact with the ground, at least four contacts points are17

needed, with as many force variables and contact constraints.18

It is then very costly to handle several bodies in contact. In19

the following, we focus on the case of planar rigid contact,20

and propose a reduced formulation such that the cost of the21

HQP does not increase linearly with the number of points in22

contact.23

V. REDUCED FORMULATION OF RIGID PLANAR CONTACTS24

Instead of considering one variable per contact forcef , the25

contact forces are summarized by the generalized 6D (spatial)26

force exerted by the body contacting the environment.27

Aq + b+ J⊤c φ = ST τ (30)

whereJc is now the Jacobian of the contacting body expressed28

on any arbitrary fixed pointc of the body, andφ is the 6D force29

(linear and angular components) expressed atc. The contact is30

supposed to occur between two rigid planar surfaces, one of31

them being a face of one robot body, the other one belonging32

to the environment. If the robot is in contact with two or more33

planar surfaces at the same time, several planar contacts are to34

be considered. The pointc denotes the arbitrary origin of the35

reference frame attached to the robot body in contact (c can36

be on the contact surface as before or anywhere on the contact37

body, e.g. on the last joint). A rigid planar contact is defined38

by at least three unaligned points of the bodypi, i = 1..l39

(l ≥ 3), that define the boundaries of the contact polygon. For40

i = 1..l, fi denotes the contact force applied topi. The vector41

f of the contact forcesfi is related toφ by:42

φ =

[ ∑i fi∑

i pi × fi

]= X

f1...fl

= Xf (31)

with

X =

[I I ... I

[p1]× [p2]× ... [pl]×

]

where the first three components ofφ are the linear part43

of the force vector, the second three components are the44

angular part and[pi]× is the cross-product matrix defined45

by [pi]×z = pi × z for any vectorz. Using this notation,46

the necessary and sufficient condition to ensure the contact47

stability (in the sense that the contact remains in the same48

phase of the complementary condition, i.e. no take off) is that 49

all the normal componentsf⊥i of the contact forcesfi are 50

positive, expressing the fact that the reaction forces of the 51

surface are directed toward the robot: 52

f⊥ ≥ 0 (32)

with f⊥ = Snf = (f⊥1 , f⊥2 , . . . , f

⊥l ) the vector of the normal 53

components of the forces at the contact points andSn the 54

matrix selecting the normal components. 55

A. Including the contact forces within the QP Solver 56

Condition (32) must now be introduced in the HQP pro-57

posed at the end of Section IV-B 58

1) A first way of modeling the problem:The constraints 59

should be written with respect to the optimization variables, 60

while (32) depends onf . A first way of writing (32) with 61

respect to the optimization variables is to use the linear map 62

X betweenφ and f given by (31). In order to compute 63

f , (31) should be inverted by using a particular generalized64

inverseX#: 65

f = X#φ (33)

The normal componentf⊥ is then given by: 66

f⊥ = SnX#φ = Fφ (34)

The condition of positivity off⊥ is then written with respect 67

to the optimization variables: 68

Fφ ≥ 0 (35)

The resulting HQP is (30)≺ (22) ≺ (35) ≺ (14), with the 69

vector of optimization variables beingu = (q, τ, φ). 70

However, it is possible to show that (35) is only a sufficient71

condition of (32), that is too restrictive. In fact, the mapX 72

is not invertible. Thus, by choosing a specific inversion.#, 73

an unnecessary assumption is made, and it may happen that74

an admissibleφ produces a negativef⊥ = S⊥X#φ. Fig. 2 75

displays the domain reached by the center of pressure: for a76

necessary and sufficient condition, the whole support polygon 77

should be reached. Using the 2-norm, only the included78

diamond is reached, as presented in Fig. 2. Various included79

quadrilaterals are reached when using other norms for the80

inversion operator#. 81

2) Using contact forces as variables:The problem is that 82

the forcesfi cannot be uniquely determined fromφ, while it 83

is possible to determineφ from fi. To cope with this problem 84

we propose to include the contact forcesf in the optimization 85

variables of the QP resolution. Condition (32) is then directly 86

written with respect to the variablesu = (τ, q, φ, f), with the 87

HQP: (30)≺ (22) ≺ (31) ≺ (32) ≺ (14). 88

Compared to the HQP formulated at the end of Sec-89

tion IV-B, this new formulation considerably reduces the size 90

of Jc, and thus the whole complexity of the resolution scheme.91

Adding φ inside the variables acts as a proxy on the bigger-92

dimension variablef . The contact forces only appear for the93

positivity condition (32) and for the link withφ (31). The 94


-0.04

-0.02

0

0.02

0.04

0.06

-0.05 0 0.05 0.1

-0.04

-0.02

0

0.02

0.04

0.06

-0.05 0 0.05 0.1

-0.04

-0.02

0

0.02

0.04

0.06

-0.05 0 0.05 0.1

-0.04

-0.02

0

0.02

0.04

0.06

-0.05 0 0.05 0.1

Fig. 2. Random sampling of the reached support region. The actual supportpolygon is the encompassing rectangle. The point clouds display the ZMPof random forces admissible in the sense of (35). Random forces φ areshot and the correspondingf = X#φ are computed. Ifφ respects (35),the corresponding center of pressure is drawn. Each sub-figure displays theadmissible forces for a different weighted inversion (the Euclidean norm isused on the top left, and random norms for the three others). Only a sub-region of the support polygon can be reached, experimentallyillustrating thefact that (35) is a too-restrictive sufficient condition.

HQP is now sparse on the column corresponding tof , which1

could be optimally exploited only if the solver is sparse. Inthe2

following, we rather propose to reduce the formulation while3

making the constraint matrix dense.4

3) Reducing the size of the variablef : It is possible5

to decouple in (31) the link betweenφ and the tangent6

components off . φ was previously expressed at an arbitrary7

point c of the contact body (φ = cφ). Consider the pointo8

chosen at the interface of contact (e.g.o is the projection9

of c on the contact surface).oφ denotes the 6D forces ato,10

expressed in terms ofoφ as follows:11

oφ =

[foτo

]=

[I3 03

[oc]× I3

]cφ = oXc

cφ (36)

with ox the coordinates of any quantityx in the frame12

Fo centered ato, having its z-axis normal to the contact13

surface. From (31) and (36), it comes:14

ofx =∑

i fxi = cfx

ofy =∑

i fyi = cfy

ofz =∑

i fzi = cfz

oτx =∑

i −opzi fyi +

∑iopyi f

zi = −czcfy + cτx

oτy =∑

i −opxi fzi +

∑iopzi f

xi = czcfx + cτy

oτz =∑

i −opyi fxi +

∑iopxi f

yi = cτz

(37)Sinceo is coplanar with thepi, theopzi are null. The previous15

expression reveals a decoupling incφ: the forcesofx,y and16

the torqueoτz are expressed in terms offx,yi . The forceofz17

and the torquesoτx,y are a function offzi . In the QP,ofx,y18

andoτz are unconstrained and can be removed along with the19

associated constraints (37.1), (37.2) and (37.6). The reduced20

rigid-contact constraint can be expressed as follows:21

Qc

[φf⊥

]= 0

f⊥ ≥ 0

(38)

with 22

Qc =

0 0 −1 0 0 0 1 1 . . . 10 cz 0 −1 0 0 py1 py2 . . . pyl

−cz 0 0 0 −1 0−px1−px2 . . .−pxl

The HQP is then (30)≺ (22) ≺ (38) ≺ (14) with the 23

optimization variables:u = (τ, q, φ, f⊥). 24

B. Generalization to multiple contacts 25

Eq. (30) considers one single body in contact. If several bod- 26

ies are in contact or one body is in contact with several planes, 27

a forceφi is introduced for each couple plane-body in contact:28

Aq + b+∑

i

J⊤i φi = ST τ (39)

For each body in contact, the same reasoning can be applied29

separately. Support polygons and normal forcesf⊥i have to 30

be introduced. For each contact,f⊥i is constrained to be 31

positive and can be linked toφi using (36). The corresponding 32

no-motion constraint is denoted by (22.i) and the positivity 33

constraint by (38.i). 34

C. Multiple tasks and final norm 35

Similarly, if several tasks are considered, (14.j) denotes 36

the constraint for each taskj. After adding all the tasks, 37

some DOF may remain unconstrained. In that case, it is38

desirable to comply with Gauss’ principle. This is possible39

by imposing q = a0 as the least priority, wherea0 is the 40

acceleration of the unconstrained system8. This has strictly 41

the same effect as weighting all the pseudoinverses byA−1, as 42

done in (17) [56]. However, in the corresponding DOF, there is 43

no damping mechanism that would reduce the motion energy44

and stabilize the system. The task-function formalism requires 45

the system to be fully constrained to ensure its stability in46

terms of automatic control (Lyapunov stability, [13]). On a47

physical robot, damping is always present. For perfect systems 48

like simulations, where damping is absent or is perfectly49

compensated, it is better to introduce, at the very last level, a 50

task to cope with the case of an insufficient number of tasks51

and constraints to fulfill the full-rank condition: 52

q = −Kq (40)

Various full-rank constraints could have been considered (min- 53

imum acceleration, distance to a reference posture, etc). The 54

choice of using the minimum velocity constraint is arbitrary. 55

Finally, the complete HQP forn contacts andk tasks is 56

written: (39) ≺ (22.1) ≺ (38.1) ≺ ... ≺ (22.n) ≺ (38.n) ≺ 57

(14.1)≺ ... ≺ (14.k) ≺ (40), with the optimization variables 58

u = (τ, q, φ1, f⊥1 , ..., φn, f

⊥n ). 59

D. Opening to other classes of contacts 60

The model (22)-(38) is built on the rigid point contact.61

From the basic point model, many other variations can be62

built. In particular, it is straightforward to obtain edge contact. 63

Elastic contact can be defined by modifying the equation64

8Similarly, the constraint can be imposed on a least-squareτ .


of motion (22) [41]. Linearized friction cones can also be1

considered, by replacingJ⊤c f by J⊤

c Gλ and f⊥ ≥ 0 by2

λ ≥ 0, whereG is a family of generators of the linearized3

cone, andλ are the multipliers of these generators [38],4

[39]. Motions with slips are made possible by removing the5

motion constraint (22) in the tangent directions, and setting a6

constraint on the tangent force to be outside the friction cone.7

However, the limitation in the viewpoint of real-time control8

is the size of the obtained QP formulation: typically, a good9

cone approximation is obtained with twelve generators, which10

introduce twelve new variables per point of contact. The11

prospectives of this work for humanoid robot control are to12

find reduced formulations to handle these situations. In the13

remaining of the paper, the reduced rigid planar formulation14

is used, since it keeps a relatively low computational cost while15

covering many possible situations with the humanoid robot.16

VI. CONTROL LAW ROBUSTNESS17

A. Comparison of(38) with the ZMP condition18

A classical situation is to have one or two feet of the19

humanoid robot in contact with a flat horizontal floor. In this20

case, a classical condition to enforce the contact stability is21

to check that the ZMP stays inside the support polygon [63],22

[64]. In this section, this condition is proved to be equivalent23

to (32).24

Proposition VI.1. In the case of contact with a horizontal25

floor, the rigid contact condition(32) is equivalent to the well-26

known contact stability condition which requires that the ZMP27

belongs to the support polygon.28

1) Sufficient implication:As above, the robot is supposed29

to be in single support9. The contact surface is supposed to30

be horizontal. The ZMP (also called center of pressure –31

COP [65]) can be defined as the barycenter of the contact32

points pi delimiting the contact surface of the foot with a33

horizontal floor, weighted by the normal componentf⊥i of34

the contact forcesfi at these points10:35

z =1

Σif⊥iΣipif

⊥i (41)

In affine geometry, it is well known that the convex hull of36

a polygon can be written as the set of all positive-weight37

barycenters of the vertices [66]. The rigid contact condition38

defined by (32) ensures that eachf⊥i is positive. Consequently,39

(32) together with (41) ensures that the ZMP belongs to the40

convex hull of the contact pointspi which, by definition, is41

equal to the support polygon.42

2) Necessary implication:On the other hand, if the ZMP43

belongs to the support polygon, there always exists a distri-44

bution of contact forcesfi at the pointspi, having positive45

componentsf⊥i , and such that the ZMP is the barycenter of46

the pi weighted by thef⊥i . This is sometimes referred to as47

weak contactstability [62] for which the ZMP is known to be48

9The same reasoning holds with several bodies in contact with the samehorizontal plane.

10The foot is usually a rectangle but any shape delimited by three or morecontact points can be considered as well.

a reduced condition [67]. When the support polygon is defined49

by more than three contact points (l > 3), an infinite number 50

of possible barycenter weightsf⊥i can be found to define the 51

ZMP. For given weights, one of thef⊥i can be negative (this 52

is typically what happens in Fig. 2). However, since the ZMP53

is inside the convex hull, there is at least one combination of 54

non-negative weights that reaches it. 55

B. Brief stability analysis 56

The inverse dynamics schemes are known to be sensitive to57

modeling errors [68]. In particular, if the inertia parameters are 58

not perfectly known, the application of the reference torques 59

will lead to different accelerations. The estimated value of X 60

is denoted byX. The solution of the QP is equivalent to the61

solution given by the pseudoinverse if none of the positive-62

force constraints are active; otherwise, it has a similar form 63

with an additional projection and can be written for one task: 64

τ = (JA−1PfST )+(e∗ + µ) (42)

where Pf is the projection operator onto the contact zero-65

motion constraint (22) and onto the set of contact positive-66

force constraints (38) that are active. Using (26), the observed 67

task acceleration when applying this control law, also denoted 68

by ., is: 69

e = JA−1PcST (JA−1PfS

T )+(e∗ + µ)− µ (43)

SincePcPf = Pf , e = e∗ if all the estimations are perfect. 70

If the estimations are biased, applying the control (42) in71

closed-loop at the whole-body level is known to keep the72

stability properties of the control lawe∗ in the task space 73

iff JA−1PcST (JA−1PfS

T )+ is definite positive [13]. When 74

the estimation error is due to an inaccurate dynamic model, a75

classical solution to reduce the estimation error is to relyon a 76

time-delay estimation, i.e reporting the biases observed at one 77

iteration of the control on the next iteration [69]. However, this 78

technique cannot perfectly cancel the errors of estimation, thus 79

(43) still holds. 80

The referencee∗ is not perfectly tracked. It is also true 81

for the contact forces computed by the solver. Indeed, the82

observed forces are: 83

φ = (JcT )#AJT

c φ∗ + (JcA

−1JcT )−1JcA

−1Aq∗ (44)

where φ∗ and q∗ are the reference force and acceleration84

computed by the HQP andJcq is neglected. The second term85

is close to0 when A is not too far fromA. Similarly, the 86

first term is nearly the identity matrix when the estimation is 87

correct. The previous equation can be summarized by: 88

φ = (I + ǫ1)φ∗ + ǫ2q

∗ (45)

with ǫ1 and ǫ2 two matrices that tend to zero when the89

estimation tends to perfection. When theǫi are not null, 90

the observed forceφ is biased with respect to the solver 91

predictions. If the bias is too great, there is no guarantee92

that the observed forceφ will maintain the contact; then the 93

property of stability can be lost. 94

In conclusion, applying the computed torques in closed-loop 95

ensures the stability of the control as long as the observed96

forces respect the contact positivity-force constraint. 97


C. The contact condition as a qualitative robustness indicator1

The previous stability analysis is not very instructive in2

practice, since it is barely possible to predict when the ob-3

servedφ will keep the contact stability. The robustness of the4

control scheme thus relies on the behavior ofφ. It is interesting5

to provide an indicator of how easy it is forφ to leave6

the acceptable domain. When considering one single point in7

contact as in Section IV, this indicator is straightforwardto8

choose: consider the normal force valuef⊥∗ computed by the9

solver. If f⊥∗ is large, then for smallǫ1, ǫ2, we can be very10

confident thatf⊥ will be positive and keep the contact stable.11

Then, for one single contact point, the positivity off⊥∗ is a12

good indicator of the robustness of the control.13

For more than one single contact point, it is not possible to14

use a direct combination of the normal forces as an indicator.15

Indeed there is an infinite number of possible force values,16

all of them being equivalent in terms of the robot behavior.17

Once more, this is linked to the results displayed in Fig. 2:18

the computed solution may include one zero normal force,19

while another solution exists with strictly positive values.20

When considering a single planar contact, the ZMP is a good21

indicator of robustness: when the predicted ZMPz∗ is far22

inside the support polygon, then we can be very confident23

that the observed ZMPz will stay inside the support polygon,24

which means in return that all thef⊥ are positive.25

If the contacts are not coplanar, the ZMP is not defined.26

In that case, the generalized zero-momentum point (GZMP)27

[70] has been proposed. Contrary to the ZMP or to (38), the28

GZMP is not a constructive criterion, i.e. it has not been used29

to generate a motion or a control law. The idea of the GZMP30

is to find from the 3D contact points a plane that will act31

like the floor plane for the ZMP. On this plane, all the force32

boundaries are projected, defining a 2D polygon. The GZMP33

exists in this same plane. The contact-stability criterionsays34

that the GZMP should remain inside the 2D polygon. The35

GZMP is easy to display. It is easy to visualize the distance36

to the boundaries and thus to have a qualitative evaluation37

of the motion robustness with respect to the contact stability.38

The GZMP needs some implementation work in order to be39

calculated, since the 2D projection plane is deduced from ge-40

ometrical computations. Moreover, it is only an approximated41

criterion, since the friction forces are neglected. To copewith42

these limitations and obtain a generative criterion, the GZMP43

was augmented in [67]. However, this last criterion, like (38),44

cannot be easily plotted, and is thus not relevant to judge the45

robustness of the obtained motion.46

Consider the six first rows of the dynamic equation. The47

dependency onτ disappears:48

Aq + b = Jcf (46)

whereA, b and Jc are the first six rows of respectivelyA, b49

andJcT . For a givenq∗, the left term is constant, denoted by50

ψ∗. It corresponds to the actuation of the free-floating body51

that cannot be accomplished by the motors. The variablef can52

be partitioned in two partsf = (f♥, f♠): f♥ is unconstrained,53

while f♠ is subject to the positivity constraint.Jc is similarly54

partitioned intoJ♥ andJ♠. The setK♠ := {ψ = J♠f, f > 0}55

is a 6D cone, that can be expressed by its facets. The motion56

is robust to the parameter error if the pointψ∗ − J♥f♥ = 57

(I − J♥J♥+)ψ∗ is deep inside the cone. The distance from58

this point to the closest facet ofK♠ can be used as a measure59

of the robustness of the motion. In the following, this criterion 60

is referred to as robustness criterion VI-C. 61

VII. E XPERIMENTS 62

Three sets of experiments are presented in this section. The63

first one presents a simple oscillatory motion that illustrates 64

the saturation of the contact-stability constraints. The second 65

one presents a complex sequence of tasks to make the robot sit66

in an armchair using several successive contacts. This motion 67

is also executed by the real robot. The last experiment presents 68

a dynamic transition of contacts. First, the setup is detailed. 69

A. Experimental setup 70

The inverse formulation of the dynamic equation of mo-71

tion (30) is given to the HQP solver. However, since it72

computes explicitly bothτ and q, it solves simultaneously 73

the forward and inverse dynamics of the robot. Both values74

can then be used as control input. The accelerationq can 75

be integrated in simulation, or provided as control inputs to 76

the robot servo control; or the torques can be given as the77

robot control, or provided to a dynamics simulator. On current 78

humanoid robots, such as HRP-2, only the first solution is79

possible11. However, this solution has the drawback that the80

servo will be on the position variables, while, as explained81

in the previous section, the robustness mainly relies on the82

accuracy of the force variables. In simulation, both solutions 83

are possible. The second solution is more beneficial, since it 84

makes it possible to double-check the dynamic computations. 85

In practice, we have used this last solution. The dynamic86

simulator AMELIF [73] was used to resolve the forward87

dynamics from the computed torquesτ∗. The simulator checks 88

the collision, computes the acceleration from the collision set 89

and the torque input using a linear solver and numerically90

integratesq using a classical Runge-Kutta of the fourth order.91

The current set of contacts is then provided to the control92

solver, along with the current position and velocity of the93

robot. The control is updated every1ms. It is computed using 94

the control framework SOT [34] and the dedicated solver [27]. 95

The result of this simulation is a joint trajectory of the robot, 96

that complies to the multi-body dynamics. This trajectory can 97

then be replayed on the real robot, using a classical position- 98

control mode. 99

The task set used in the three presented motions is the100

following. A first task function is used to control the position 101

and orientation of one operational point of the robot (e.g.102

grippers, head, chest). The task error is the positionp and 103

angle-vector orientationrθ [74] of the operational point with 104

respect to a referencep∗, rθ∗ expressed in the world frame: 105

eop =

[p− p∗

rθ ⊖ uθ∗

](47)

11The second solution will be possible with the next humanoid robotgeneration, e.g. Romeo [71] or DLR [72].


The reference acceleration is computed from this error as a1

proportional-derivative (PD) control law:2

e⋆op = −λpeop − λdeop (48)

where eop = Jopq is the velocity in the task space and the3

gainsλp, λd are used to tune the convergence velocity (usually,4

λd = 2√λp). For tracking a moving target, a fixed high gain5

is used forλp. When reaching a fixed target, an adaptive gain6

is typically used:7

λp : ||e|| → (λ0 − λ∞)eβ||e|| + λ∞ (49)

whereλ0 is the gain when the error is null,λ∞ is the gain far8

from the target, andβ adjusts the switching behavior between9

the gains. A typical setting is(λ0, λ∞, β) = (450, 15, 100). A10

second taskegaze is used to servo the projections of one point11

of the environment on the right camera plane to a reference12

positions∗ [14]:13

egaze = s− s∗ (50)

The reference acceleratione∗ is also defined by (48). The14

torque magnitude is also bounded. Since the torques are15

included in the vector of optimization variables, it is trivial to16

express the torque limits by a simple bound on these variables:17

τ ≤ τ ≤ τ (51)

with τ = −τ the maximum torque value.18

Similarly, bounds have to be set on the joint positions. Since19

the positions are not variables of the solver, the constraint is20

set on the joint accelerations:21

q ≤ q + TS q +TS

2

2q ≤ q (52)

whereq andq denote the lower and upper joint-limits respec-22

tively, andTS is the length of the preview windows. In theory,23

the control sampling time∆T = 1ms should be used forTS .24

In practice, a smoother behavior can be obtained by adjusting25

this valueTS := ∆Tλs

whereλs can be tuned as the gain of the26

task. We usedλs = 0.1 to generate the following motions.27

B. Experiment A: Swing posture28

1) Description: The objective of this experiment is to29

validate the contact stability constraint. It is inspired by a30

biomechanics experiment which aims at testing the human31

swinging posture behavior with respect to the same con-32

straints [75]. A tracking task is imposed to the robot head33

to make it oscillate. Depending on the frequency and the34

amplitude of the oscillation, forces are obtained at the contact35

points, that may saturate the contact constraint. The task36

ehead given by (47) is imposed to the head operational point,37

where only the translation on the forward axis is selected.38

The reference position is given by a time-varying sinusoid,39

around a central pointxc = 0.02 and with amplitude of40

5cm and frequency0.3Hz (low frequency),0.56Hz (medium41

frequency) or0.9Hz (high frequency). The gain is set to42

λp := 250 to ensure good tracking. The complete SOT is:43

(39) ≺ (22) ≺ (38) ≺ (51) ≺ (52) ≺ ehead ≺ (40).44

In theory, the contact points are defined from the 3D model45

of the robot. However, in practice, we never consider the real46

support polygon, but a smaller one. This simple trick ensures 47

increased robustness of the motion when trying to replay it48

on the robot. For example, on the feet, the support polygon is49

often defined as a square of 4cm centered below the ankle50

axis [76], [77]. The obtained robustness can be evaluated51

afterward with respect to the real support polygon. 52

The motion is played four times. In the first two executions,53

both feet are flat on the ground and the reference is oscillating 54

at low and medium frequencies respectively. For the next two55

executions, the right gripper contact is added and the motion 56

is played at medium and high frequencies. In the following,57

the four motions are referred to as2pt-low, 2pt-medium, 3pt- 58

mediumand3pt-high respectively. 59

2) Results:The experiment is summed up by Figures 3 to60

6. The motion is displayed in Fig. 3. The robot is oscillating61

forward and backward to follow the head reference. The62

two motions2pt-low and 2pt-mediumwere already detailed 63

in [1] where the plots of joint positions and torques can be64

found. When only the feet are contacting, the stability of the65

motion can be evaluated by displaying the ZMP, plotted in66

Fig. 4. At low frequency, the ZMP does not saturate because67

the demanded accelerations are small enough. At medium68

frequency, the accelerations are larger and the ZMP saturates. 69

Since the real support polygon is about 20cm wide, there is70

a large offset that ensures a good robustness when executing71

this motion on the real robot. 72

The robustness can be evaluated using the criterion proposed 73

in Section VI-C. The contact constraints of the solver are74

projected into the space of the spatial forces expressed at the 75

waist point. Then the distance of the pointψ∗ (46) to this 76

constraint set is computed. The result is plotted in Fig. 5. First, 77

the distance is computed to the constraint set of the solver (the 78

4cm-wide support polygon). As expected, the distance is null79

when the ZMP saturates. More interestingly, the distance can 80

be computed to the real constraints by taking into account the 81

true polygon as well as the linearized friction cones at the82

contact points. The friction coefficient was set toK = 0.5. In 83

that case, the robustness criterion is always strictly positive, 84

showing that the motion is robust to small perturbations or85

model uncertainties. 86

Using only the feet as contacts, it is not possible to follow87

the reference at high velocity. A third contact point is added to 88

increase the stability domain. The contact polygon is a square 89

of 5cm centered at the gripper terminal point. Contrary to90

the ZMP, the robustness criterion VI-C is still valid with non- 91

coplanar contacts. When the friction cones are not considered 92

(slidingless contact), it is always possible to find a set of93

contact forces following a given CoM acceleration (the system 94

is said to be inforce closure[78]). In that case, the distance to 95

the constraint set is always infinite. The robustness criterion 96

is finite when the friction cones are considered. The friction 97

coefficient at the gripper is set toK = 0.1. At medium 98

frequency, the motion can be considered as very robust since99

the criterion is always very far from 0. If the frequency is in- 100

creasing, the criterion remains smaller. It then jumps fromone 101

constraint edge to another, which explains the discontinuities. 102

The computation time depends on the number of contacts,103

tasks and active constraints as shown in Fig. 6. 104


t=1.2s t=1.7s t=2.1s

Fig. 3. Experiment A: Top: Snapshots of the oscillatory movement 2pt-medium. Bottom: Feet and ZMP positions at the corresponding instants. TheZMP saturates on the front when the robot is reaching its top amplitude anddecelerates to go backward. Similarly, the ZMP saturates on the back.

0.5 1 1.5 2 2.5 3 3.5 4

−0.02

−0.01

0

0.01

0.02

time (s)

ZM

P x

−po

sitio

n (m

)

low frequency

medium frequency

ZMP limit

Fig. 4. Experiment A: ZMP position along the forward (x) axis for thetwo motions with only the feet contacts. The support polygon is a4cm-widesquare centered on the ankle joint. The ZMP does not saturatewhen themotion oscillates at low frequency. It saturates at medium frequency.

0 0.5 1 1.5 2 2.5 30

5

10

15

20

25

30

35

40

45

time (s)

Dis

tanc

e to

con

e

low 2pt solver

low 2pt real

medium 2pt solver

medium 2pt real

3pt medium

3pt high

Fig. 5. Experiment A: robustness criterion VI-C. For the firsttwo motions2pt-low and 2pt-medium, the criterion is given with respect to the supportpolygon defined in the solver (small contact surface) in bold,and with respectto the real support polygon taking into account the frictioncone (linearized bytwelve facets) in nonbold. This criterion behaves similarlyto the distance ofthe ZMP to the support polygon. The criterion is plotted for3pt-mediumand3pt-high. If the solver support polygons are considered, the distance is infinite.It is only plotted for the distance to the friction cone.

0.5 1 1.5 2 2.5 3

8.5

9

9.5

10

10.5

time (s)

Com

puta

tion

time

(ms)

low 2pt

med 2pt

med 3pt

high 3pt

Fig. 6. Experiment A: Computation time. For the motion2pt-medium,the saturation of the force constraints clearly induces an increase of thecomputation cost, whereas for2pt-low the cost remains constant. For3pt-medium, the cost is constant (no saturation) but higher in average due to theadditional contact. Finally, the cost of3pt-high is higher and varies when theconstraints are saturated.

C. Experiment B: sitting in the armchair 1

1) Description: The second experiment illustrates the pos-2

sibilities of multiple non-coplanar contacts during a more3

complex sequence of motion. The robot sits in an armchair4(see Fig. 7). First, contacts of the left then right grippers5

are found with the armrests to increase the contact stability 6

domain. Then, the pelvis is brought in contact with the seat.7At the highest priority of the stack, the limits (51) and (52) 8

ensure that the joints and actuator limits are respected. Two 9

tasks erh and elh, defined by (47), are set on each robot10

gripper to control the position and orientation toward the11

corresponding armrest. To prevent a collision when grasping, 12

an intermediate point is first reached, above the grasping13

position. The contact of each gripper with the armrest is14

realized by the rear part of the opened gripper. The support15

polygon is then a5cm-wide square. To improve the naturalness16

of the motion, a taskegaze defined by (50) is set to constrain 17

the gaze toward the armrest to be grasped. After each grasp,18

the gaze is brought back in front of the robot. Finally, the waist 19

is controlled by a taskewaist also defined by (47) where only 20

the vertical position and sagittal rotation are active: thewaist 21

is constrained to remain vertical and to move down to the seat. 22

The complete SOT is defined by: (39)≺ (22)≺ (38)≺ (51)≺ 23

(52) ≺ ehand ≺ egaze ≺ ewaist ≺ (40), with ehand being the 24

right or left hand task, when active. The temporal sequence of 25

tasks is given in Fig. 8. Essentially, the robot looks left and 26

bends to grasp the left handle; then it looks right and bends27

to grasp the right handle; finally, using both handle supports, 28

it moves the pelvis down to sit. 29

2) Results:The experiment is summarized in Figures 7 to30

13. The key frames of the motion executed by the robot are31

given in Fig. 7. The sequence of tasks is summarized in Fig. 8.32

On each of the following figures, the chronological sequence33

is recalled by vertical stems at the transition instants. During 34

the motion, the joint range is extensively used. The most35

representative joint trajectories are plotted in Fig. 9. The neck 36

joint reaches its limit while looking left. In reaction, allthe 37

other aligned joints move to overrun the neck limitation (chest 38

joint of course, but also hip and ankle joints). The right hip39

then reaches its limit. In consequence, all the motions of both 40

legs are stopped, due to a lack of DOF to compensate this limit. 41

The chest joint absorbs all the subsequent overrun to fulfill42

the task. Again, the neck joint reaches its limit when looking 43

right. This time, the velocity of the joint when it reaches its 44

limit is higher, which leads to a strong acceleration of the45

chest, and consequently brings the neck out of its limit. This 46

behavior could be damped if necessary by tuningλs in (52). 47

The chest joint finally reaches its limit at the end of the right- 48

grasp task, which produces a limited overrun on the other49

joints. All the joints are properly stopped at the limit, andcan 50

leave the neighborhood of the limit without being stuck as it51

may appear with some avoidance techniques. 52

The contact with the two armrests is very useful to control53

the descent of the waist. The vertical forces on each support54

are plotted in Fig. 10. In the beginning, the weight is fully55

supported by the two feet, as shown on Fig. 11. Aftert = 8s, 56

the left arm is used to sustain the robot. However, the robot57


t=0s t=7s t=15s t=19s

Fig. 7. Experiment B: Snapshots of the motion executed on the real HRP-2 robot. The robot is standing on both feet (t = 0s). It first looks left and graspsthe left armrestt = 7s. It then looks right, grasps the right armrest (t = 15s) and finally sits (t = 19s).

0 5 10 15 20 25

rh e

lh e

gazee

waiste

time (s)

pre−grasp grasp contact

pre−grasp grasp contact

left center right center

down

Fig. 8. Experiment B: Sequence of tasks and contacts. The gazetask focusessequentially on the left and right armrests and on a virtual point in front ofthe robot. The pre-grasp tasks are set at the vertical10cm above the graspposition.

0 5 10 15 20 250

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

time (s)

Nor

mal

ized

join

t pos

ition

R.hip

R.ankle

L.hip

L.ankle

Pan.chest

Tilt.chest

Pan.neck

Fig. 9. Experiment B: normalized joint position (0 and 1 are resp. the lowerand upper limits) of the right and left hip and ankle, chest andneck joints.The joint limits are properly avoided. When a limit is reached, one or severaljoints move in reaction to overcome the saturation.

0 5 10 15 20 250

50

100

150

200

250

300

350

400

time (s)

Nor

mal

forc

es (

N)

L.footR.footL.gripR.grip

Fig. 10. Experiment B: Vertical forces distribution.

−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2−0.1 −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4

com

x c

oord

inat

e (m

)

com y coordinate (m)

Left FootRight Foot

First partSecond partThird part

Fig. 11. Experiment B: Position of COM. The three phases correspond tochanges in the number of contacts (first the two feet, then the left gripper andfinally both feet and grippers). Firstly, the COM stays forward, but is finallymoved backward to reach the second armrest and move the pelvis down tothe seat.

0 5 10 15 20 250

5

10

15

time (s)

Dis

tanc

e to

the

cone

Fig. 12. Experiment B: Robustness criterion VI-C. The distance is computedwith respect to the friction cones. The friction coefficientat the armrests isroughly estimated to be 5 times less than at the sole. The less robust partoccurs during the final phase, where the waist moves down.

0 5 10 15 20 2518

20

22

24

26

28

time (s)

Com

puta

tion

time

(ms)

Fig. 13. Experiment B: Computation time.


upper body is still in front of the chair, and this contact is not1

fully used yet. In order to reach the second armrest, the robot2

has to move its weight back (see Fig. 11) and use the left-arm3

contact to ensure its balance: nearly half of the weight is then4

supported by the arm. Finally, the right armrest is grasped,and5

the robot distributes its weight on the four contacts equally.6

Neither the center of mass (COM) nor the ZMP can give a7

proper estimation of the stability, since the motion is neither8

quasi-static nor supported by planar contacts. The robustness9

estimator presented in Section VI-C is plotted in Fig. 1210

with respect to the linearized friction cones at both feet and11

grippers. The motion is very stable, except at the end of the12

motion, when the waist moves down. At that time, the robot is13

using the tangent forces of the grippers on the armrest, which14

nearly saturates the friction cone. In consequence, this part of15

the motion is the less robust when executed by the real robot.16

Indeed, since the armrests do not respect the hypothesis of17

rigid contact and due to this lack of robustness, it can be18

observed that the toes nearly leave the ground during this19

phase of the motion. This effect is very interesting, since it20

confirms the relevance of the robustness criterion. Of course,21

this undesirable effect could be avoided by setting a more22

accurate model of the environment or adding a safety limit to23

the positivity constraint in the solver.24

Finally, the computation times are plotted in Fig. 13. The25

SOT is nearly full. In that case, the computation cost is around26

20ms per iteration, i.e. five times the real-time if controlling27

the robot at200Hz. The computation cost depends on the28

number of tasks and even more on the number of contacts, as29

shown by the computation increase att = 8s and t = 18s.30

D. Experiment C: Dynamic contact transition31

1) Description: At the beginning of the motion, the robot32

is standing on both feet and its COM is artificially pushed33

forward using a task on its chest. The robot is then out of its34

domain of quasi-static stability: the only solution to restore35

the balance is to change the set of supports. The two grippers36

(first the left, then the right) are then sent forward to establish37

a contact with the wall, in order to increase the set of support38

contacts and to restore the balance. An overview of the motion39

is given in Fig. 14. Three tasks of type (47) are used: one task40

on the chest, that controls only the translation; another one41

on each gripper controls both the translation and the rotation.42

The COM is not explicitly controlled. The sequence of tasks43

and contacts is given in Fig. 15.44

2) Results:The experiment is summarized in Fig. 14 to 18.45

If using only quasi-static movements (i.e. reaching while keep-46

ing the COM inside the feet support polygon), the maximal47

reaching distance of HRP-2 is around85cm. In this motion,48

the wall is positioned1m in front of the robot, as shown in49

Fig. 14. The motions of the COM along with the forward50

direction are plotted in Fig. 16. The COM quickly leaves the51

support polygon in the beginning of the motion, due to the52

artificial motion of the chest. Fromt = 0.7s, the COM is53

out of the support polygon with a positive velocity: it is then54

impossible to bring it back to stability without changing the55

supports. The balance is restored aftert = 2.5s, with the COM56

coming back to zero velocity. The stability is evaluated using 57

the robustness criterion presented in Section VI-C. When only 58

the feet are in contact, the ZMP is at the forward limit of the59

support polygon, which corresponds to a low robustness. The60

robustness increases when the first gripper enters into contact. 61

However, at that time, the tangent forces of the gripper on62

the wall are high. The robot can then lose its balance by63

rotating on one of the gripper-foot edges, as already observed 64

in [70]. The second gripper helps to improve the stability65

by decreasing the tangent forces at each contact point. The66

vertical forces are plotted in Fig. 18. On the grippers, the67

vertical corresponds to a direction tangent to the contact.68

Betweent = 1.9s and t = 2.5s, the tangent forces at the 69

left gripper are high, at the limit of the friction cones, which 70

corresponds to a weaker robustness of the motion (the gripper 71

is close to slide). 72

VIII. C ONCLUSION 73

This paper proposes a complete solution to perform task-74

space (operational-space) inverse dynamics while taking into 75

account various tasks, unilateral constraints such as joint 76

position or torque limits and preserving the contact stability. 77

Complex motions can be composed from several tasks, con-78

straints and contacts, by ensuring a strict hierarchy between 79

conflicting references. Several models of unilateral contacts 80

can be considered. The most usual one is the rigid point81

contact. We have also proposed a reduced formulation to82

express rigid planar contacts. The contact condition has been 83

shown to be equivalent to theZMP-inside-the-support-polygon 84

constraint in the particular case of the humanoid robot standing 85

on a flat floor. To quantify the quality of the generated motion86

in terms of distance to the contact-stability constraints,a 87

generic criterion has been proposed, that can handle the rigid 88

slidingless point contact, the rigid planar contact, but also 89

friction cones. 90

The effectiveness of the approach has been demonstrated by91

generating different motions for the humanoid HRP-2. These92

motions have been generated off-line because the motion-93

generation algorithm is close to but still not real-time. They are 94

fully consistent with the robot dynamics and can be replayed95

directly by the robot, as it was shown by making the real96

HRP-2 sit down in an armchair. 97

The future of this approach would be to apply the algorithm98

directly on the robot as a closed-loop control. This would99

require technical contributions to accelerate the solver compu- 100

tation cost, but also to consider an effective dynamic sensor- 101

based control. 102

APPENDIX A 103

GENERALIZED INVERSE 104

The notationQ# denotes any reflexive generalized inverse105

of Q [47], i.e. that respects the two first conditions of Moore-106

Penrose: 107

QQ#Q = Q108

Q#QQ# = Q#


t=0.0s t=1s t=2s t=4s

Fig. 14. Experiment C: Snapshots of the dynamic contact transition.

0 0.5 1 1.5 2 2.5 3 3.5 4

rh e

lh e

cheste

time (s)

intermediate task contact


front



front



front break

Fig. 15. Experiment C: Sequence of tasks and contacts. On eachgripper, anintermediate point is used to ensure that the final contact motion is performedalong the normal to the wall. The contact polygons of the feet and grippersare the same as previously.

0 0.5 1 1.5 2 2.5 3 3.5 40.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

time (s)

CO

M x

−ax

is (

m)

Fig. 16. Experiment C: Trajectory of the COM along the X-axis (forwarddirection). The grey rectangle marks the limit of the foot support. The COMstarts inside the support polygon, quickly leaves it when the chest is thrownforward and finally converges to a fixed position when the grippers contactthe wall and stabilize the motion.

0 0.5 1 1.5 2 2.5 3 3.5 40

2

4

6

8

10

12

time (s)

Dis

tanc

e to

the

cone

Fig. 17. Experiment C: Robustness criterion VI-C. The distance is computedwith respect to the friction cones. The friction coefficientof the gripper withthe wall is set to the same value than at the sole contact.

0 0.5 1 1.5 2 2.5 3 3.5 40

100

200

300

400

500

600

700

time (s)

Ver

tical

forc

es

L.foot

R.foot

L.hand

R.hand

Fig. 18. Experiment C: Vertical forces (normal forces for the feet, tangentforces for the gripper).

In general,Q# is chosen among the possible inverses as the1one which minimizes the norm in both the task space and2the control parameter (referred to as the pseudoinverse in3the paper, and denoted by.+); i.e. it verifies the two second 4

conditions of Moore-Penrose: 5

QQ# is symmetrical.6

Q#Q is symmetrical.

Alternatively, one of these two (or both) conditions can be7

relaxed to impose a different metric in the task space or on8the control parameter. In particular, a weighted generalized 9

inverse [46] can be chosen to impose a given minimumR- 10

norm in the control space||u||2R = uTRu, whereR is a 11

given symmetric positive definite matrix; in that case, the12

inverse is given byQ#R =√R(Q

√R)+ = RQT (QRQT )+, 13

where√R is any decomposition such that

√R

T√R = R, for 14

example the Choleski decomposition. A weighted generalized 15

inverse can also impose a minimumL-norm in the task 16

space ||e∗ − Qu||2L = (e∗ − Qu)TL(e∗ − Qu); in that 17

case, it isQL# = (√LQ)+

√L = (QTLQ)+QTL. Of 18

course, bothR and L norms can be imposed byQL#R = 19√R(

√LQ

√R)+

√L. 20

APPENDIX B 21

HQP COMPLEXITY 22

Consider a HQP whose variablex is dimensionn, whose 23

constraints have the following form:Ax ≤ b. The choice of an 24

active setA defines an equality-only HQP (eHQP), with fewer25

constraints whose form areAix = bi, whereAi (resp.bi) are 26

the rows ofA (resp.b) selected byA. The eHQP solution can 27

be computed by a set of pseudoinverses following (2). The28

active-search algorithm [52], [27] uses a heuristic to find the 29

optimal active set, for which the eHQP computes the optimal30

x. The algorithm is presented in Alg. 1, see [27] for more31

details. 32

Basically, the eHQP routine costso(mn2) where m is 33

the number of rows of the problem, which is approximately34

o(n3) when the eHQP is nearly square. Ifp is the number 35

of iterations in the loop Row#3, then the complexity12 is 36

roughly o(pn3). 37

In the case presented in this paper, the HQP is called at38

each iteration of the robot. Between two iterations, the HQP39

values vary slightly. Because the HQP is a continuous function 40

of the constraints, the active set also varies slightly. If using 41

the optimal active set of the previous robot time to initialize 42

the current active search, then the number of HQP iterationsp 43

remains small (experimentally,p is null 99% of the time, and 44

is never more than10). 45

Consider now the robotics HQP presented in the paper. The46

number of DOF is denoted byn. In the inverse-kinematics 47

HQP, the size of the variable is the number of DOF of48

the systemn, and the robot is generally nearly completely49

constrained. The cost of the inverse kinematics by HQP is50

thuso(pn3), with experimentallyp < 10. 51

12In fact, only the first eHQP call is ino(n3), the following ones can beupdated foro(n2).


Algorithm 1 HQP active search1: Input: A, b,A0

2: A := A0

3: repeat4: Ai := AA, bi := bA5: x∗, w := eHQP (Ai, bi)6: if ∃k /∈ A, wk > 0, then7: A+ = k8: continue9: end if

10: if ∃k ∈ A, wk < 0, then11: A− = k12: continue13: end if14: until never15: return x∗

For inverse dynamics withq the size of the contact variable,1

the cost iso(p(2n + q − 6)3). For the reduced planar model2

when only the feet are in contact,q = 20, which makesN =3

36 + 30 + 20 for the HQP variable.4

APPENDIX C5

PROOF OF EQUIVALENCE6

We prove in the following the equivalence between the7

scheme proposed in Section IV and the control law proposed8

in [59].9

a) Control scheme:We recall first the development of the10

operational-space control law for dynamic systems under rigid11

contacts [59]. The task Jacobian knowing a set of contacts is12

defined by:13

Jt|c = JPcT (53)

where the subscriptt|c indicates that the task space14

quantities are projected in the space consistent with15

the contact constraints. By left multiplying (25) by in-16

verse transpose constrained Jacobian(Jt|c#A−1

)T =17 (A−1Jt|c

T (Jt|cA−1Jt|c

T )−1)T

, the task-space dynamic evo-18

lution is obtained:19

Λt|ce + µt|c = Qt|cST τ (54)

with Λt|c = (Jt|cA−1Jt|c

T )−1, Qt|c = (Jt|c#A−1

)TPc and20

µt|c = Qt|cb + (Jt|c#A−1T

JcT (JcA

−1JcT )−1Jc − Λt|cJ)q.21

The control torques that perform the reference task are directly22

computed by numerical inverting (54):23

τ∗ = ((Jt|c#A−1

)TPcST )#f∗

= J⋆T f∗(55)

where24

J⋆ = Jt|c(SPcT )#

and25

F = Λt|ce + µt|c

This final equation corresponds to the standard force-to-joint-26

torque mapping, linking the end-effector forcesf∗ to the joint27

torques by the transpose of the Jacobian of the robot.28

b) Proof of equivalence:Control law (55) can be shown 29

to be equivalent to the control law proposed in Section IV.30

On the one hand, sinceSPTc is full row rank, (55) can be 31

rewritten: 32

τ∗ = (SPT

c A−1

PcST )−1

SPTc A

−1PcJ

T (JPTc A

−1PcJ

T )−1e∗

(56)On the other hand, the scheme proposed in Section IV can33

be written: 34

τ = (JA−1PcST )#W e∗ (57)

with W a user-defined weight matrix. Developing the weightedinverse gives [46]:

τ =WSPTc A

−1JT (JA−1PcSTWSPT

c A−1JT )−1e∗

The weight is chosen toW = (SA−1PcST )−1 =

(SPTc A

−1PcST )−1 [57]. Since A−1Pc = PT

c A−1 =

PTc A

−1Pc [59], the equivalence between (56) and (57) isbrought to prove that:

JA−1PcST (SA−1PcS

T )−1SPTc A

−1JT = (JPTc A

−1PcJT )

We can recognize the term (SPTc )#A−1

= 35

A−1PcST (SA−1PcS

T )−1 in the previous equality. It 36

thus reduces to: 37

J(SPTc )#A−1

SPTc A

−1JT = (JPTc A

−1PcJT ) (58)

In [59], it is proved that(SPTc )#SPT

c = PTc , which concludes 38

the proof. 39

REFERENCES 40

[1] L. Saab, N. Mansard, F. Keith, J.Y. Fourquet, and P. Soueres. Generation 41

of dynamic motion for anthropomorphic systems under prioritized 42

equality and inequality constraints. InIEEE Int. Conf. on Robotics and 43

Automation (ICRA’11), Shangai, China, 2011. 44

[2] L. Saab, O. Ramos, N. Mansard, P. Soueres, and J.Y. Fourquet. Generic 45

dynamic motion generation with multiple unilateral constraints. In 46

IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS’11), San 47

Francisco, USA, September 2011. 48

[3] P. Varkonyi, D. Gontier, and J. Burdick. On the lyapunov stability of 49

quasistatic planar biped robots. InIEEE Int. Conf. on Robotics and 50

Automation (ICRA’12), Saint Paul, USA, May 2012. 51

[4] K. Bouyarmane, A. Escande, F. Lamiraux, and A. Kheddar. Potential 52

field guide for humanoid multicontacts acyclic motion planning. In 53

IEEE Int. Conf. on Robotics and Automation (ICRA’09), Kobe,Japan, 54

May 2009. 55

[5] T. Chang and R. Dubey. A weighted least-norm solution based scheme 56

for avoiding joints limits for redundant manipulators.IEEE Trans. on 57

Robotics and Automation, 11(2):286–292, April 1995. 58

[6] A. Escande, S. Miossec, and A. Kheddar. Continuous gradient proximity 59

distances for humanoids free-collision optimized-posturesgeneration. In 60

IEEE-RAS Int. Conf. on Humanoid Robots (Humanoid’07), Pittsburgh, 61

USA, November 2007. 62

[7] E. Marchand and G. Hager. Dynamic sensor planning in visual servoing. 63

In IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS’98), 64

Leuven, Belgium, May 1998. 65

[8] L. Kavraki, M. Kolountzakis, and J.C. Latombe. Analysis of prob- 66

abilistic roadmaps for path planning.IEEE Trans. on Robotics and 67

Automation, 14(1):166–171, February 1998. 68

[9] J. Kuffner and S. LaValle. Rrt-connect: An efcient approach to single- 69

query path planning. InIEEE Int. Conf. on Robotics and Automation 70

(ICRA’00), San Francisco, USA, April 2000. 71

[10] D. Berenson, S. Srinivasa, and J. Kuffner. Task space regions: A 72

framework for pose-constrained manipulation planning.Int. Journal of 73

Robotics Research, 30(12):1435–1460, October 2011. 74

[11] S. Dalibard, A. Nakhaei, F. Lamiraux, and J.P. Laumond. Whole-body 75

task planning for a humanoid robot: a way to integrate collision avoid- 76

ance. In IEEE-RAS Int. Conf. on Humanoid Robots (Humanoid’09), 77

Paris, France, December 2009. 78


[12] H. Hanafusa, T. Yoshikawa, and Y. Nakamura. Analysis andcontrol1

of articulated robot with redundancy. InIFAC, 8th Triennal World2

Congress, volume 4, pages 1927–1932, Kyoto, Japan, 1981.3

[13] C. Samson, M. Le Borgne, and B. Espiau.Robot Control: the Task4

Function Approach. Clarendon Press, Oxford, United Kingdom, 1991.5

[14] B. Espiau, F. Chaumette, and P. Rives. A new approach to visual servo-6

ing in robotics.IEEE Trans. on Robotics and Automation, 8(3):313–326,7

June 1992.8

[15] F. Chaumette and S. Hutchinson. Visual servo control, part ii: Advanced9

approaches.IEEE Robotics and Automation Magazine, 14(1):109–118,10

March 2007.11

[16] N. Mansard and F. Chaumette. Task sequencing for sensor-based control.12

IEEE Trans. on Robotics, 23(1):60–72, February 2007.13

[17] J. Rosen. The gradient projection method for nonlinear programmimg,14

part i, linear constraints. SIAM Journal of Applied Mathematics,15

8(1):181–217, March 1960.16

[18] A. Li egeois. Automatic supervisory control of the configuration and17

behavior of multibody mechanisms.IEEE Trans. on Systems, Man and18

Cybernetics, 7(12):868–871, December 1977.19

[19] Y. Nakamura. Advanced Robotics: Redundancy and Optimization.20

Addison-Wesley Longman Publishing, Boston, 1991.21

[20] B. Siciliano and J-J. Slotine. A general framework for managing multiple22

tasks in highly redundant robotic systems. InIEEE Int. Conf. on23

Advanced Robotics (ICAR’91), Pisa, Italy, June 1991.24

[21] P. Baerlocher and R. Boulic. An inverse kinematic architecture enforcing25

an arbitrary number of strict priority levels.The Visual Computer,26

6(20):402–417, August 2004.27

[22] O. Khatib. Real-time obstacle avoidance for manipulators and mobile28

robots. Int. Journal of Robotics Research, 5(1):90–98, March 1986.29

[23] D. Raunhardt and R. Boulic. Progressive clamping. InIEEE Int. Conf.30

on Robotics and Automation (ICRA’07), Roma, Italy, April 2007.31

[24] C. Chevallereau and W. Khalil. A new method for the solution of32

the inverse kinematics of redundant robots. InIEEE Int. Conf. on33

Robotics and Automation (ICRA’88), pages 37–42, Philadelphia, USA,34

April 1988.35

[25] A. Hofmann, M. Popovic, and H. Herr. Exploiting angular momentum to36

enhance bipedal center-of-mass control. InIEEE Int. Conf. on Robotics37

and Automation (ICRA’09), Kobe, Japan, 2009.38

[26] O. Kanoun, F. Lamiraux, F. Kanehiro, E. Yoshida, and Laumond J-P.39

Prioritizing linear equality and inequality systems: application to local40

motion planning for redundant robots. InIEEE Int. Conf. on Robotics41

and Automation (ICRA’09), Kobe, Japan, May 2009.42

[27] A. Escande, N. Mansard, and P-B. Wieber. Fast resolution of hierar-43

chized inverse kinematics with inequality constraints. InIEEE Int. Conf.44

on Robotics and Automation (ICRA’10), Anchorage, USA, May 2010.45

[28] O. Khatib. A unified approach for motion and force controlof robot46

manipulators: The operational space formulation.International Journal47

of Robotics Research, 3(1):43–53, February 1987.48

[29] F.E. Udwadia and R.E. Kalaba. A new perspective on constrained49

motion. Proceedings: Mathematical and Physical Sciences, pages 407–50

410, 1992.51

[30] A. Laulusa and O. Bauchau. Review of classical approaches for52

constraint enforcement in multibody systems.Journal of Computational53

and Nonlinear Dynamics, 3, 2008.54

[31] O. Bauchau and A. Laulusa. Review of contemporary approaches for55

constraint enforcement in multibody systems.Journal of Computational56

and Nonlinear Dynamics, 3, 2008.57

[32] J. Peters, M. Mistry, F. E. Udwadia, J. Nakanishi, and S.Schaal. A58

unifying framework for robot control with redundant DOFs.Autonomous59

Robots, 24(1):1–12, January 2008.60

[33] J. Nakanishi, R. Cory, M. Mistry, J. Peters, and S. Schaal. Operational61

space control: A theoretical and empirical comparison.Int. Journal of62

Robotics Research, 27(6):737–757, June 2008.63

[34] N. Mansard, O. Stasse, P. Evrard, and A. Kheddar. A versatile gen-64

eralized inverted kinematics implementation for collaborative working65

humanoid robots: The stack of tasks. InIEEE Int. Conf. on Advanced66

Robotics (ICAR’09), Munich, Germany, June 2009.67

[35] J. Park and O. Khatib. Contact consistent control framework for68

humanoid robots. InIEEE Int. Conf. on Robotics and Automation69

(ICRA’06), Orlando, USA, May 2006.70

[36] O. Khatib, L. Sentis, and J. Park. A unified framework for whole-body71

humanoid robot control with multiple constraints and contacts. In Eu-72

ropean Robotics Symposium, pages 303–312, Prague, Czech Republic,73

March 2008.74

[37] N. Mansard, O. Khatib, and A. Kheddar. Integrating unilateral con-75

straints inside the stack of tasks.IEEE Trans. on Robotics and76

Automation, 25(3), May 2009.77

[38] C. Collette, A. Micaelli, C. Andriot, and P. Lemerle. Dynamic balance 78

control of humanoids for multiple grasps and noncoplanar frictional 79

contacts. InIEEE-RAS Int. Conf. on Humanoid Robots (Humanoid’07), 80

Pittsburgh, USA, December 2007. 81

[39] K. Bouyarmane and A. Kheddar. Multi-contact stances planning for 82

multiple agents. InIEEE Int. Conf. on Robotics and Automation 83

(ICRA’11), Shangai, China, May 2011. 84

[40] S. Boyd and B. Wegbreit. Fast computation of optimal contact forces. 85

IEEE Trans. on Robotics, 23(6):1117–1132, December 2007. 86

[41] J. Park and O. Khatib. A haptic teleoperation approach based on contact 87

force control. Int. Journal of Robotics Research, 25(5):575–591, 2006. 88

[42] M. Vukobratovic and D. Juricic. Contribution to the synthesis of biped 89

gait. IEEE Trans Biomed Eng., 16(1):1–6, January 1969. 90

[43] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, 91

and H. Hirukawa. Biped walking pattern generation by using preview 92

control of zero-moment point. InIEEE Int. Conf. on Robotics and 93

Automation (ICRA’03), Taipei, Taiwan, September 2003. 94

[44] Y. Nakamura and H. Hanafusa. Inverse kinematics solutions with 95

singularity robustness for robot manipulator control.ASME Journ of 96

Dyn. Sys., Measures and Control, 108:163–171, September 1986. 97

[45] E. Moore. On the reciprocal of the general algebraic matrix. Bulletin 98

of the American Mathematical Society, 26(9):394–395, 1920. 99

[46] K. Doty, C. Melchiorri, and C. Bonivento. A theory of generalized 100

inverses applied to robotics.Int. J. of Robotics Research, 12(1):1–19, 101

December 1993. 102

[47] A. Ben-Israel and T. Greville. Generalized inverses: theory and 103

applications. CMS Books in Mathematics. Springer, 2nd edition, 2003.104

[48] G. Golub and C. Van Loan. Matrix computations. John Hopkins 105

University Press, 3rd edition, 1996. 106

[49] A. Deo and I. Walker. Robot subtask performance with singularity 107

robustness using optimal damped least squares. InIEEE Int. Conf. on 108

Robotics and Automation (ICRA’92), pages 434–441, Nice, France, May109

1992. 110

[50] T. Sugihara. Solvability-unconcerned inverse kinematics by the 111

levenberg-marquardt method.IEEE Trans. on Robotics, 25(3):658–669, 112

June 2011. 113

[51] S. Chiaverini. Singularity-robust task-priority redundancy resolution 114

for real-time kinematic control of robot manipulators.IEEE Trans. on 115

Robotics and Automation, 13(3):398–410, June 1997. 116

[52] R. Fletcher. A general quadratic programming algorithm.IMA Journal 117

of Applied Mathematics, 7:76–91, February 1971. 118

[53] S. Boyd and L. Vandenberghe.Convex Optimization. Cambridge 119

University Press, 2004. 120

[54] C. Maes.A Regularized Active-Set Method for Sparse Convex Quadratic 121

Programming. PhD thesis, Institute for Computational and Mathematical122

Engineering, Stanford University, USA, November 2010. 123

[55] R. Featherstone.Rigid body dynamics algorithms. Springer, 2008. 124

[56] H. Bruyninckx and O. Khatib. Gauss’ principle and the dynamics of 125

redundant and constrained manipulators. InIEEE Int. Conf. on Robotics 126

and Automation (ICRA’00), San Fransisco, USA, April 2000. 127

[57] J. Park.Control Strategies for Robots in Contact. PhD thesis, Stanford 128

University, USA, March 2006. 129

[58] O. Khatib, L. Sentis, J. Park, and J. Warren. Whole body dynamic 130

behavior and control of human-like robots.Int. Journal of Robotics 131

Research, 1(1):29–44, March 2004. 132

[59] L. Sentis.Synthesis and Control of Whole-Body Behaviors in Humanoid133

Systems. PhD thesis, Stanford University, USA, July 2007. 134

[60] N. Mansard. A dedicated solver for fast operational-space inverse 135

dynamics. InIEEE Int. Conf. on Robotics and Automation (ICRA’12), 136

St Paul, USA, May 2012. 137

[61] L. Sentis, J. Park, and O. Khatib. Compliant control of multicontact and 138

center-of-mass behaviors in humanoid robots.IEEE Trans. on Robotics, 139

26(3):483 – 501, April 2010. 140

[62] J. Pang and J. Trinkle. Stability characterizations ofrigid body contact 141

problems with coulomb friction.ZAMM-Journal of Applied Mathematics 142

and Mechanics/Zeitschrift fur Angewandte Mathematik und Mechanik, 143

80(10):643–663, 2000. 144

[63] S. Kajita, H. Hirukawa, K. Harada, and K. Yokoi.Introduction a la 145

commande des robots humanoıdes. Translated from the Japanese original146

version by S. Sakka. Springer, 2009. 147

[64] M. Vukobratovic and B. Borovac. Zero moment point - thirty fives years 148

of its life. Int. Journal of Humanoid Robotics, 1(1):157–173, January 149

2004. 150

[65] P. Sardain and G. Bessonnet. Forces acting on a biped robot. center of 151

pressure-zero moment point.IEEE Transactions on Systems, Man and152

Cybernetics (Part A), 34(5):630–637, September 2004. 153


[66] M. Audin. Geometry, pages 29, Proposition 5.6. Translated from the1

1998 French original version. Springer-Verlag, 2002.2

[67] H. Hirukawa, S. Hattori, K. Harada, S. Kajita, K. Kaneko, F. Kanehiro,3

K. Fujiwara, and M. Morisawa. A universal stability criterion of the4

foot contact of legged robots-adios zmp. InIEEE Int. Conf. on Robotics5

and Automation (ICRA’06), 2006.6

[68] K. Youcef-Toumi and O. Ito. A time delay controller for systems with7

unknown dynamics. InIEEE American Control Conference, 1988, pages8

904–913, 1988.9

[69] P.H. Chang and J.W. Jeong. Enhanced operational space formulation for10

multiple tasks by using time-delay estimation.IEEE Trans. on Robotics,11

2012. To be published.12

[70] K. Harada, S. Kajita, K. Kaneko, and H. Hirukawa. Dynamics and13

balance of a humanoid robot during manipulation tasks.IEEE Trans.14

on Robotics, 22(3):568–575, June 2006.15

[71] E. Guizzo. France developing advanced humanoid robot romeo. IEEE16

Spectrum Automaton Blog, December 13, 2010.17

[72] C. Ott, C. Baumgartner, J. Mayr, M. Fuchs, R. Burger, D. Lee,18

O. Eiberger, A. Albu-Schaffer, M. Grebenstein, and G. Hirzinger.19

Development of a biped robot with torque controlled joints. In IEEE-20

RAS Int. Conf. on Humanoid Robots (Humanoid’10), Paris, France,21

December 2010.22

[73] P. Evrard, F. Keith, J.-R. Chardonnet, and A. Kheddar. Framework for 23

haptic interaction with virtual avatars. InIEEE Int. Symp. on Robot and 24

Human Interactive Communication, Munich, Germany, August 2008. 25

[74] Y. Ma. An invitation to 3-d vision: from images to geometric models, 26

volume 26, chapter 2: Representation of three-dimensionnal moving 27

Scene. Springer Verlag, 2004. 28

[75] V. Bonnet, P. Fraisse, N. Ramdani, J. Lagarde, S. Ramdani,and B. Bardy. 29

Modeling postural coordination dynamics using a closed-loop controller. 30

In IEEE-RAS Int. Conf. on Humanoid Robots (Humanoid’08), Daejeon, 31

Korea, December 2008. 32

[76] A. Herdt, H. Diedam, P.B. Wieber, D. Dimitrov, K. Mombaur, and 33

M. Diehl. Online walking motion generation with automatic footstep 34

placement.Advanced Robotics, 24(5-6):719–737, 2010. 35

[77] A. Escande, A. Kheddar, S. Miossec, and S. Garsault. Planning 36

support contact-points for acyclic motions and experiments on hrp-2. In 37

International Symposium on Experimental Robotics (ISER’08), Athens, 38

Greece, July 2008. 39

[78] C. Ferrari and J. Canny. Planning optimal grasps. InIEEE Int. Conf. 40

on Robotics and Automation (ICRA’92), 1992. 41

submitted to ieee transaction on robotics 1 dynamic whole...

Documents