HAL Id: lirmm-00831097
https://hal-lirmm.ccsd.cnrs.fr/lirmm-00831097
Submitted on 6 Jun 2013
HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entic research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.
L’archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diusion de documents
scientiques de niveau recherche, publiés ou non,
émanant des établissements d’enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.
Dynamic Whole-Body Motion Generation Under Rigid
Contacts and Other Unilateral Constraints
Layale Saab, Oscar Efrain Ramos Ponce, François Keith, Nicolas Mansard,
Philippe Souères, Jean-Yves Fourquet
To cite this version:
Layale Saab, Oscar Efrain Ramos Ponce, François Keith, Nicolas Mansard, Philippe Souères, et al..
Dynamic Whole-Body Motion Generation Under Rigid Contacts and Other Unilateral Constraints.
IEEE Transactions on Robotics, IEEE, 2013, 29 (2), pp.346-362. �10.1109/TRO.2012.2234351�.
�lirmm-00831097�
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-
body motions on a humanoid robot accounting for various tasks
and constraints is the inverse kinematics. Based on the task-
function approach, this class of methods makes possible the
coordination of the robot movements to execute several tasks in
parallel and account for the sensor feedback, in real-time thanks
to the low computation cost. To some extent, it also enables
dealing with some of the robot constraints (e.g. joint limits or
visibility) and managing the quasi-static balance of the robot. In
order to fully use the whole range of possible motions, this paper
proposes to extend the task-function approach to handle the
full dynamics of the robot multi-body along with any constraint
written as equality or inequality of the state and control variables.
The definition of multiple objectives is made possible by ordering
them inside a strict hierarchy. Several models of contact with the
environment can be implemented in the framework. We propose
a reduced formulation of the multiple rigid planar contact that
keeps a low computation cost. The efficiency of this approach is
illustrated by presenting several multi-contact dynamic motions
in simulation and on the real HRP-2 robot.
Index Terms—Humanoid robotics, redundant robots, dynam-
ics, force control, contact modeling.
I. INTRODUCTION
T
HE GENERATION of motion for humanoid robots is
a challenging problem, due to the complexity of their
tree-like structure and the instability of their bipedal posture
[3]. Typical examples are shown in Fig. 1, with the HRP-
2 robot using multiple non-coplanar contacts to perform a
dynamic motion. These robots own a large number of degrees
of freedom (DOF), typically more than 30. In return, they are
subject to various sets of constraints (balance, contact, actuator
Manuscript received July 19, 2012; accepted December 11, 2012. This
paper was recommended for publication by Associate Editor Y. Choi and
Editor B. J. Nelson upon evaluation of the reviewers comments. This paper
was presented in part at the IEEE International Conference on Robotics
and Automation in 2011 [1] and the IEEE/RSJ International Conference on
Intelligent Robots and Systems in 2011 [2].
L. Saab was with the Laboratory for Analysis and Architecture of Sys-
tems, Centre National de la Recherche Scientifique, University of Toulouse,
Toulouse 31077, France. She is now with EOS Innovation,
´
Evry 91000, France
(e-mail: layalesaab@gmail.com).
O. E. Ramos, N. Mansard, and P. Sou
`
eres are with the Laboratory for
Analysis and Architecture of Systems, Centre National de la Recherche Sci-
entifique, University of Toulouse, Toulouse 31077, France (e-mail: oramos@
gmail.com; nicolas.mansard@laas.fr; soueres@laas.fr).
F. Keith is with the Laboratoire dInformatique de Robotique et de Micro
´
electronique de Montpellier, Centre National de la Recherche Scientifique,
Montpellier 34095, France (e-mail: francois.keith@lirmm.fr).
J-Y. Fourquet is with the Laboratoire G
´
enie de Production, de l
´
Ecole
Nationale d Ing
´
enieurs de Tarbes, University of Toulouse, Tarbes 65016,
France (e-mail: fourquet@enit.fr).
Color versions of one or more of the figures in this paper are available
online at http://ieeexplore.ieee.org.
Digital Object Identifier 10.1109/TRO.2012.2234351
Fig. 1. Dynamic multi-contact motion with the HRP-2 model.
limits), that reduce the space of possible motions. These
constraints can typically be formulated as equalities (e.g. zero
velocity at rigid-contact points [4]), and inequalities (e.g.
joint position [5], velocity or torques bounds, obstacles [6]).
Moreover, they are of relative importance (e.g. balance has
to be considered more important than visibility [7]). In total,
the motion has to be designed in a set that lives in the
high-dimensional configuration space but is implicitly limited
to a much smaller submanifold by the set of constraints.
This makes the classical sampling methods [8], [9] more
difficult to use than for a classical manipulator. The motion
manifold cannot be sampled directly but by projection [10].
The connection process in high-dimension is costly [11] and
and often fails due to the number of constraints.
Rather than designing the motion at the whole-body level
(configuration space), the task function approach [12], [13]
proposes to design the motion in a space dedicated to the
task to be performed. It is then easier to design the reference
motion in the task space, and transcripting this reference from
the task space to the whole-body level is only a numerical
problem. This approach is versatile, since the same task is
generally transposable from one robot or situation to another.
It also eases the use of sensory feedback, since the sensory
space is often a good task-space candidate [14], [15].
A task is a basic brick of motion, which can be combined
sequentially [16] or simultaneously to a complex motion.
Simultaneous execution can be achieved in two ways: by
weighting, or by imposing a strict hierarchy. Coming from nu-
merical optimization [17], this second solution was introduced
in robotics by [18] and formalized for any number of tasks
in [19], [20]. This approach is well fitted to cope with equality
constraints. However, inequality constraints cannot be taken
into account explicitly. Therefore, approximate solutions, such
as potential field approaches [21], [7] or damping functions
[5], [22] have been proposed to consider inequalities.
The transcription of the motion reference from the task
space to the whole-body control is naturally written as a
Submitted to IEEE Transaction on Robotics 2
quadratic program (QP) [23]. A QP is composed of two layers,
namely the constraint and the cost. It can be seen as a hierarchy
of two levels, the constraint having priority over the cost. If
only equality constraints are considered, the QP resolution
corresponds to the inversion schemes [20], in the particular
case of two levels. Inequalities can also be taken into account
directly, as constraints, or in the cost function [24]. In [25], a
method to extend the QP formulation to any number of priority
levels is given. The solution of such a hierarchical problem is
computed by solving a cascade of QP (or hierarchical QP).
In [26], a dedicated solver has been proposed to obtain the
solution at lower cost (in one step instead of a cascade).
All these works only consider the kinematics of the robot.
On a humanoid robot, many constraints arise from the dynam-
ics of the multi-body system. The formulation by task can be
extended to compute the torque at the whole-body level from
the reference motion expressed in the dedicated task space,
also called operational space [27]. For a humanoid in contact,
the motion is constrained to the submanifold of configurations
that respects the contact model [28] as illustrated by Fig. 1. A
review of the work in modeling and control of the dynamics
of a set of bodies in contact is proposed in [29], [30]. The
connection with inverse dynamics has been done in [31], [32].
Using these approaches, it is possible to take into account
a hierarchy of tasks and constraints (or stack of tasks [33]),
all written as equalities [34], [35]. In [36], a first solution to
handle inequalities in the stack of tasks was proposed, but
cannot set any inequality constraint on the contact forces.
In [37], [38], the inverse-dynamics problem has been written
as a QP, where the unilateral contact constraints, along with
classical unilateral constraints (joint limits, etc) are explicitly
considered. In that case, several tasks can be composed by
setting relative weights, but a hierarchy of tasks is not possible.
In this paper, we propose a generic solution to take into
account equalities and inequalities in a strict hierarchy to
generate a dynamic motion. This solution is based on the sim-
ilarities between inverse kinematics and inverse dynamics. In
Section II, the inverse-kinematics scheme is recalled, written
into a general form; the possibility of taking into account
inequalities is then introduced using the solver [25], [26].
Then, putting the operational-space inverse dynamics under
the same generic form, Section III uses the same hierarchical
solver to take into account both dynamics and inequalities.
This first solution deals with the robot in free space. In
Section IV, contacts are introduced in the model and used in
the resolution scheme. The contact model is generic and can be
adapted to various situations (rigid contact, friction cone [39],
elastic contact [40]). A solution is proposed in Section V to
implement a reduced form of multiple plane/plane slidingless
rigid contacts. In Section VI the connection is made with the
zero-moment point (ZMP) contact criterion [41] classically
used in humanoid robotics [42]. The generation is close to the
real time (around 20ms per control cycle on a typical 30DOF
robot). Some examples of complex motions involving non-
coplanar contacts and their execution on the real robot are
presented in Section VII.
II. INVERSE KINEMATICS
A. The task-function approach
The task-function approach [13], or operational-space ap-
proach [27], [43], provides a mathematical framework to
describe tasks in terms of specific output functions. The task
function is a function from the configuration space to an
arbitrary task space, chosen to ease the observation and the
control of the motion with respect to the task to perform.
A task is defined by a triplet (e, ˙e
∗
, Q), where e is the task
function that maps the configuration space to the task space,
˙e
∗
is the reference behavior expressed in the tangent space to
the task space at e. Q is the differential mapping between the
task space and the control space of the robot which verifies
the relation:
˙e + µ = Qu (1)
where u is the control in the configuration space and µ is the
drift of the task. To compute a specific robot control u
∗
that
performs the reference ˙e
∗
, any numerical inverse of Q can be
used. The generic expression of the control law is then :
u
∗
= Q
#
( ˙e
∗
+ µ) + Pu
2
(2)
In this expression, the first part performs the task, and the
second part, modulated by the secondary control input u
2
,
expresses the redundancy of the task [18]. In the first term,
Q
#
is any reflexive generalized inverse of Q, often chosen to
be the (Moore-Penrose) pseudoinverse Q
+
[44] or a weighted
inverse Q
#W
[45] (see App. A). In the second term of (2),
P = I − Q
#
Q is the projector onto the null space of Q
corresponding to Q
#
.
B. Hierarchy of tasks
The projector P is intrinsically related to the redundancy
of the robot with respect to the task e. A secondary task
(e
2
, ˙e
∗
2
, Q
2
) can be executed using u
2
as a new control input.
Introducing (2) in ˙e
2
+ µ
2
= Q
2
u gives:
˙e
2
+ ˜µ
2
=
f
Q
2
u
2
(3)
with ˜µ
2
= µ
2
− Q
2
Q
#
( ˙e
∗
+ µ) and
f
Q
2
= Q
2
P. This last
equation fits the template (1), and can be solved using the
generic expression (2) [20]:
u
2
∗
=
f
Q
2
#
( ˙e
∗
+ ˜µ
2
) + P
2
u
3
(4)
where P
2
enables the propagation of the redundancy to a third
task using the input u
3
. By recurrence, this generic scheme can
be extended to any arbitrary hierarchy of tasks.
C. Inverse kinematics formulation
In the inverse-kinematics problem, the control input u is
simply the robot joint velocity ˙q. The differential map Q
between the task and the control is the task Jacobian J. In
that case, the drift µ =
∂e
∂t
is often null, and (1) is written:
˙e = J ˙q (5)
Submitted to IEEE Transaction on Robotics 3
The simplest and most-often used solution is to choose Q
#
to
be the pseudoinverse Q
+
, that gives the least Euclidean norm
of both ˙q and ˙e
∗
− J ˙q [46], [47]. The control law is then:
˙q
∗
= ˙q
∗
1
+ P ˙q
2
(6)
where ˙q
∗
1
= J
+
˙e
∗
. A typical reference behavior is an expo-
nential decay of e to zero: ˙e
∗
= −λe, λ > 0.
It may happen that J becomes singular, i.e. rank(J) < r
0
,
where r
0
is the nominal rank of J out of the singular config-
uration. Numerical problems can occur during the transition
from the nominal situation to the singular one. To avoid these
problems, the pseudoinverse is often approximated by the
damped least-square J
†
defined by [48], [49]:
J
†
=
J
ηI
+
I
0
(7)
where I is the identity matrix of proper size and η is a
damping factor, chosen as an additional parameter of the
control (typically, η = 10
−2
for a humanoid robot).
D. Projected inverse kinematics
Consider a secondary task (e
2
, ˙e
∗
2
, J
2
). The template (3) is:
˙e
2
− J
2
˙q
∗
1
= J
2
P ˙q
2
(8)
In this case, the differential map is the projected Jacobian
Q = J
2
P, and the drift is µ = −J
2
˙q
∗
1
. The control input ˙q
∗
2
is obtained once more by numerical inversion [20], [50]:
˙q
∗
2
= (J
2
P)
+
( ˙e
2
− J
2
˙q
∗
) + P
2
˙q
3
(9)
where P
2
is the projector into the null space of J
2
P . The same
scheme can be reproduced iteratively to take into account any
number of tasks until P
i
is null.
In general rank(J
2
P) ≤ rank(J
2
) ≤ r
2
, where r
2
is the
nominal rank of J
2
. When the second inequality is strict, the
singularity is said to be kinematic; when the first inequality is
strict, the singularity is said to be algorithmic [51]
1
. To avoid
any numerical problem in the neighborhood of the singularity,
a damped inverse can be used to invert J
2
P.
E. Hierarchical quadratic program resolution
1) Generic formulation: When considering a single task,
the solution obtained with the pseudoinverse (2) is known to be
the optimal solution of the QP min
u
||Qu− ˙e
∗
−µ||
2
. The great
advantage of the QP formulation is that both linear equalities
and inequalities can be considered, while the pseudoinverse-
based schemes presented above cannot explicitly deal with
inequalities. A quadratic program is composed of a quadratic
cost function to be minimized while satisfying the set of
constraints [52]. It can be seen as a two-level hierarchy, where
the set of constraints has priority over the cost. Inequalities are
set as the top-priority. The introduction of slack variables is a
classical solution to handle an inequality at the second priority
level [53]. In [25], it was proposed to use the slack variables
to generalize the QP to more than two levels of hierarchy and
1
Both cases are similar in the sense that
J
1
J
2
is singular.
thus to build a hierarchical quadratic problem (HQP) handling
inequalities.
The HQP formulation is first recalled in a generic frame.
A generic constraint k is defined by the linear map A
k
and
the two inequality bounds (b
k
,
b
k
), where b
k
and
b
k
are
respectively the lower and upper bounds on the reference
behavior
2
. At level k, the cascade algorithm solving the
hierarchy (A
k
, b
k
) is expressed by the following QP:
min
u
k
,w
k
||w
k
||
2
s.t.
b
k−1
≤ A
k−1
u
k
+ w
∗
k−1
≤
b
k−1
b
k
≤ A
k
u
k
+ w
k
≤
b
k
(10)
where A
k−1
, (b
k−1
,
b
k−1
) are the constraints at all the
previous levels from 1 to k − 1 (A
k−1
= (A
1
, ..., A
k−1
)),
and A
k
, (b
k
,
b
k
) is the constraint at level k.
The slack variable
3
w
k
is used to add some freedom to
the solver if no solution can be found when the constraint k is
introduced under the k −1 previous constraints: w
k
is variable
and can be used by the solver to relax the last constraint A
k
.
On the other hand, w
∗
k−1
is constant and set to the result
of the previous optimization of the k − 1 first QP (at each
of the iterations of the cascade, w
∗
k−1
is augmented with the
optimal w
∗
k
by w
∗
k−1
:= (w
∗
k−1
, w
∗
k
)). A solution to the strict
k − 1 constraint A
k−1
is then always reached, even if the
slack constraint A
k
is not feasible: this corresponds to the
definition of the hierarchy.
A classical method to compute the solution of a QP or HQP
relies on an active-search algorithm [52], [26] (see App. B),
which implies iterative computations of the pseudoinverse of
a subproblem of the initial QP. Since pseudoinverses are used,
the classical numerical problems can occur in the neighbor-
hood of singularities. Regularization methods that extend the
damping inverse [49] used in robotics can be applied [54].
The method proposed above is generic and can be applied
to any numerical problem written with a linear hierarchical
structure. In that case, it is referred to as HQP (or cascade of
QP) and denoted with the lexicographic order: (i) ≺ ( ii) ≺
(iii) ≺ ... which means that the constraint (i) has the highest
priority. In the following, we propose a solution to apply this
formulation to invert kinematics and dynamics. The constraints
are then the tasks defined above and the hierarchical solver is
called a stack of tasks (SOT) or hierarchy of tasks.
2) Application to inverse kinematics: When considering a
single task, inversion (6) corresponds to the optimal solution
to the problem:
min
˙q
||J ˙q − ˙e
∗
||
2
(11)
By applying the QP resolution scheme, both equalities and
inequalities can be considered. Replacing b by ˙e, the reference
part is then rewritten:
˙e
∗
≤ ˙e ≤
˙e
∗
(12)
2
Specific cases can be immediately implemented: b
k
=
b
k
in the case of
equalities and b
k
= −∞ or
b
k
= +∞ to handle single-bounded constraints.
3
w is an implicit optimization variable whose explicit computation can be
avoided when formulating the problem as a cascade. It does not appear in the
vector of optimization variables u. See [26] for details.
Submitted to IEEE Transaction on Robotics 4
For instance, in the case of two tasks with priority order e
1
≺
e
2
, the expression of the QP is given by:
min
˙q,w
2
||w
2
||
2
s.t. ˙e
∗
1
≤ J
1
˙q + w
∗
1
≤
˙e
∗
1
˙e
∗
2
≤ J
2
˙q + w
2
≤
˙e
∗
2
(13)
In robotics, when a constraint is expressed as an inequality,
it is very likely to be put as the top priority: typically, joint
limits and obstacle avoidance. Using this framework, it is also
possible to handle inequalities at the second priority level
(i.e. in the cost function). A typical case is to prevent visual
occlusion when possible, or to keep a low velocity if possible,
without disturbing the robot behavior when it is not necessary.
In the sequel, the HQP considering linear equalities and
inequalities is extended from inverse kinematics to inverse
dynamics.
III. INVERSE DYNAMICS
In this section the case of a contact-free dynamical multi-
body system without free-floating root is considered.
A. Task-space formulation
As previously stated, a task is defined by a task function e, a
reference behavior and a differential mapping. At the dynamic
level, the reference behavior is specified by the expected task
acceleration ¨e
∗
, while the control input is typically the joint
torques τ. The operational-space inverse dynamics then refers
to the problem of finding the torque control input τ that
produces the task reference ¨e
∗
, using any necessary joint
acceleration ¨q. The acceleration ¨q is then a side variable,
that does not require to be explicitly computed during the
resolution. Contrary to the case of kinematics, the mapping
between the control input τ and the task space is obtained
in two stages. First, the map between accelerations in the
configuration space and in the task space is obtained by
differentiating (5):
¨e = J ¨q +
˙
J ˙q (14)
Then, the dynamic equation of the system expressed in the
joint coordinates is deduced from the mechanical laws of
motion [55].
A¨q + b = τ (15)
where A = A(q) is the generalized inertia matrix of the
system, ¨q is the joint acceleration, b = b(q, ˙q) includes all the
nonlinear effects including Coriolis, centrifugal and gravity
forces and τ are the joint torques. The generic form (1) is
obtained by replacing ¨q in (15) with (14) [27]:
¨e −
˙
J ˙q + JA
−1
b = JA
−1
τ (16)
This equation follows the template (1) with Q = JA
−1
, µ =
−
˙
J ˙q + JA
−1
b and u = τ.
The torque τ
∗
that ensures ¨e
∗
is solved using the generic
form (2). It is generally proposed to weight the inverse by
the inertia matrix A. This weight ensures that the process
is consistent with Gauss’ principle [56], i.e. that the torques
and accelerations corresponding to the redundancy of the task
are the closest to the acceleration of the unconstrained multi-
body system. This principle can be intuitively understood by
considering the weight like a minimization of the acceleration
pseudo energy ¨q
T
A¨q [57], [31].
The redundancy can also be explicitly formulated during
the inversion, using the form (3). A SOT can be iteratively
built, with the lower-priority tasks being executed in the best
possible way without disturbing the higher priority tasks [58],
[59]:
τ
∗
= τ
∗
1
+ Pτ
2
(17)
where P =I−J
T
(JA
−1
J
T
)
+
JA
−1
is the projector in the null
space of JA
−1
and τ
∗
1
=(JA
−1
)
#A
(¨e
∗
−
˙
J ˙q+JA
−1
b).
B. Projected inverse dynamics
As before, the differential map for the projected secondary
task e
2
is obtained by replacing (17) into the robot dynamics
equation in the task space ¨e
2
−
˙
J
2
˙q + J
2
A
−1
b = J
2
A
−1
τ:
¨e
2
+ µ
2
= Q
2
τ
2
(18)
with µ
2
= −
˙
J
2
˙q + J
2
A
−1
b − J
2
A
−1
τ
∗
1
, and Q
2
= J
2
A
−1
P.
The same weighted inverse is used to invert Q
2
[58], [59].
Accordingly, any number of tasks can be added iteratively
until the projector becomes null.
The same singularities as in inverse kinematics may appear
(the dynamics in itself does not bring any new singular case,
since A is always full rank). To avoid any numerical problem,
the damped weighted inverse is generally used. As for the
kinematics, only tasks defined by equality constraints can be
taken into account using this pseudoinverse-based resolution.
To take into account inequalities, we propose to extend to the
dynamics the HQP [25] that was previously introduced for the
kinematics.
C. Application of the QP solver to the inverse dynamics
When resolving a given task e while taking into account the
dynamics, both (14) and (15) must be fulfilled. There are two
ways of formulating the QP. First, ¨q can be substituted from
(14) into (15), to obtain the single reduced equation (16). In
that case, the QP only requires to solve τ , the variable ¨q being
not explicitly computed:
min
τ
||JA
−1
τ − ¨e
∗
− µ||
2
(19)
Alternatively, (14) can be solved under the constraint (15).
Using the hierarchy notation, the HQP is thus (15) ≺ (14), or
using the standard QP notation:
min
τ,¨q,w
||w||
2
s.t. A¨q + b = τ
¨e
∗
+ w = J ¨q +
˙
J ˙q
(20)
In that case, both τ and ¨q are explicitly computed. They
constitute the vector of optimization variables u = (τ, ¨q).
QP (19) has a reduced form, but QP (20) allows any explicit
formulation using the dynamics variables. In the following,
such an exhaustive formulation is necessary to deal with the
contact.
