Submitted to IEEE Transaction on Robotics 1 Dynamic Whole-Body Motion Generation under Rigid Contacts and other Unilateral Constraints Layale Saab, Oscar E. Ramos, Franc ¸ois Keith, Nicolas Mansard, Philippe Sou` eres, Jean-Yves Fourquet Abstract—The most widely-used technique to generate whole- 1 body motions on a humanoid robot accounting for various tasks 2 and constraints is the inverse kinematics. Based on the task- 3 function approach, this class of methods makes possible the 4 coordination of the robot movements to execute several tasks in 5 parallel and account for the sensor feedback, in real-time thanks 6 to the low computation cost. To some extent, it also enables 7 dealing with some of the robot constraints (e.g. joint limits or 8 visibility) and managing the quasi-static balance of the robot. In 9 order to fully use the whole range of possible motions, this paper 10 proposes to extend the task-function approach to handle the 11 full dynamics of the robot multi-body along with any constraint 12 written as equality or inequality of the state and control variables. 13 The definition of multiple objectives is made possible by ordering 14 them inside a strict hierarchy. Several models of contact with the 15 environment can be implemented in the framework. We propose 16 a reduced formulation of the multiple rigid planar contact that 17 keeps a low computation cost. The efficiency of this approach is 18 illustrated by presenting several multi-contact dynamic motions 19 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 NTRODUCTION 23 T HE GENERATION of motion for humanoid robots is a 24 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 degrees 27 of freedom (DOF), typically more than 30. In return, they 28 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 equalities 31 (e.g. zero velocity at rigid-contact points [4]), and inequalities 32 (e.g. joint limits [5], obstacles [6], joint velocity and torque 33 within given bounds). Moreover, they are of relative impor- 34 tance (e.g. balance has to be considered more important than 35 visibility [7]). In total, the motion has to be designed in a set 36 that lives in the high-dimensional configuration space but is 37 implicitly limited to a much smaller submanifold by the set of 38 constraints. This makes the classical sampling methods such as 39 probabilistic roadmap [8] or rapidly-exploring random tree [9] 40 more difficult to use than for a classical manipulator: the set 41 of available motion cannot be sampled directly but has to be 42 L. Saab, O. Ramos, N. Mansard and P. Sou` eres are with LAAS-CNRS, Univ. Toulouse, France, {lsaab,oramos,nmansard,soueres}@laas.fr. F. Keith is with LIRMM, CNRS, Montpellier, France, [email protected]. J-Y. Fourquet is with LGP-ENIT, Univ. Toulouse, Tarbes, France [email protected]The paper was presented in part to IEEE ICRA 2011 [1] and IEEE/RSJ IROS 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 rejection is 45 high, due to the number of constraints. 46 Rather than designing the motion at the whole-body level 47 (configuration space), the task function approach [12], [13] 48 proposes to design the motion in a space dedicated to the 49 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 numerical 52 problem. This approach is versatile, since the same task is 53 generally transposable from one robot or situation to another. 54 It also eases the use of sensory feedback, since the sensory 55 space is often a good task-space candidate [14], [15]. 56 A task can be seen as a basic brick of motion. To generate 57 a complex motion on a humanoid robot, several tasks have 58 to be combined, sequentially [16] or simultaneously. The 59 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. Coming 62 from numerical optimization [17], this second solution was 63 introduced in robotics by [18], formalized for any number of 64 tasks in [19], [20] and applied in the humanoid framework 65 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 to 71 consider unilateral constraints. 72 The transcription of the motion reference from the task 73 space to the whole-body control is naturally written as a 74 quadratic program (QP) [24]. A QP is composed of two 75 layers, namely the constraint and the cost. It can be seen as 76 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
17
Embed
Submitted to IEEE Transaction on Robotics 1 Dynamic Whole ...projects.laas.fr/gepetto/uploads/Publications/2012-saab-itro.pdf · Submitted to IEEE Transaction on Robotics 2 1 particular
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
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
Submitted to IEEE Transaction on Robotics 2
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)
Submitted to IEEE Transaction on Robotics 3
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.
Submitted to IEEE Transaction on Robotics 4
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
Submitted to IEEE Transaction on Robotics 5
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.
Submitted to IEEE Transaction on Robotics 6
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
Submitted to IEEE Transaction on Robotics 7
-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
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
Submitted to IEEE Transaction on Robotics 11
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
Submitted to IEEE Transaction on Robotics 12
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.
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.
Submitted to IEEE Transaction on Robotics 13
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#
Submitted to IEEE Transaction on Robotics 14
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
intermediate task contact
front
intermediate task contact
intermediate task contact
front
intermediate task contact
intermediate task contact
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).
Submitted to IEEE Transaction on Robotics 15
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
Submitted to IEEE Transaction on Robotics 16
[12] H. Hanafusa, T. Yoshikawa, and Y. Nakamura. Analysis andcontrol1
of articulated robot with redundancy. InIFAC, 8th Triennal World2