Stability Analysis and Design of
Momentum-based Controllers for Humanoid Robots
Gabriele Nava, Francesco Romano, Francesco Nori and Daniele Pucci1
Abstract— Envisioned applications for humanoid robots call
for the design of balancing and walking controllers. While
promising results have been recently achieved, robust and
reliable controllers are still a challenge for the control commu-
nity dealing with humanoid robotics. Momentum-based strate-
gies have proven their effectiveness for controlling humanoids
balancing, but the stability analysis of these controllers is
still missing. The contribution of this paper is twofold. First,
we numerically show that the application of state-of-the-art
momentum-based control strategies may lead to unstable zero
dynamics. Secondly, we propose simple modifications to the
control architecture that avoid instabilities at the zero-dynamics
level. Asymptotic stability of the closed loop system is shown
by means of a Lyapunov analysis on the linearized system’s
joint space. The theoretical results are validated with both
simulations and experiments on the iCub humanoid robot.
I. INTRODUCTION
Humanoid robotics is doubtless an emerging field of engi-
neering. One of the reasons accounting for this interest is the
need of conceiving systems that can operate in places where
humans are forbidden to access. To this purpose, the scien-
tific community has paid much attention to endowing robots
with two main capabilities: locomotion and manipulation. At
the present day, the results shown at the DARPA Robotics
Challenge are promising, but still far from being fully reliable
in real applications. Stability issues of both low and high
level controllers for balancing and walking were among
the main contributors to robot failures. A common high-
level control strategy adopted during the competition was
that of regulating the robot’s momentum, which is usually
referred to as momentum-based control. This paper presents
numerical evidence that momentum-based controllers may
lead to unstable zero dynamics and proposes a modification
to this control scheme that ensures asymptotic stability.
Balancing controllers for humanoid robots have long
attracted the attention of the robotic community [1],
[2]. Kinematic and dynamic controllers have been com-
mon approaches for ensuring a stable robot behavior for
years [3],[4]. The common denominator of these strategies
is considering the robot attached to ground, which allows
one for the application of classical algorithms developed for
fixed-based manipulators.
At the modeling level, the emergence of floating-base
formalisms for characterizing the dynamics of multi-body
systems has loosened the assumption of having a robot
*This paper was supported by the FP7 EU project KoroiBot (No. 611909
ICT 2013.10 Cognitive Systems and Robotics)
1 The authors are with the iCub Facility department, Istituto Italiano di
Tecnologia, Via Morego 30, Genoa, Italy name.surname@iit.it
link attached to ground [5]. At the control level, instead,
one of the major complexities when dealing with floating
base systems comes from the robot’s underactuation. In fact,
the underactuation forbids the full feedback linearization of
the underlying system [6]. The lack of actuation is usually
circumvented by means of rigid contacts between the robot
and the environment, but this requires close attention to the
forces the robot exerts at the contact locations. If not reg-
ulated appropriately, uncontrolled contact forces may break
the contact, and the robot control becomes critical [7],[8].
Contact forces/torques, which act on the system as external
wrenches, have a direct impact on the rate-of-change of the
robot’s momentum. Indeed, the time derivative of the robot’s
momentum equals the net wrench acting on the system.
Furthermore, since the robot’s linear momentum can be
expressed in terms of the center-of-mass velocity, controlling
the robot’s momentum is particularly tempting for ensuring
both robot and contact stability (see, e.g.,
[9] for proper
definition of contact stability).
Several momentum-based control strategies have been im-
plemented in real applications [10],[11],[12]. The essence of
these strategies is that of controlling the robot’s momentum
while guaranteeing stable zero-dynamics. The latter objec-
tive is often achieved by means of a postural task, which
usually acts in the null space of the control of the robot’s
momentum [13],[14],[15]. These two tasks are achieved
by monitoring the contact wrenches, which are ensured to
belong to the associated feasible domains by resorting to
quadratic programming (QP) solvers [7],[8],[16]. The control
of the zero-dynamics can also be used to control the robot
joint configuration [15].
The control of the linear momentum is exploited to sta-
bilize a desired position for the robot’s center of mass. In
contrast, the choice of desired values for the robot angular
momentum is still unclear [17]. Hence, control strategies
that neglect the control of the robot’s angular momentum
have also been implemented [18]. Controlling both the robot
linear and angular momentum, however, is particularly useful
for determining the contact torques, and it has become a
common torque-controlled strategy for dealing with balanc-
ing and walking humanoids. Controlling also the angular
momentum results in a more human-like behavior and better
response to perturbations [19],[20].
To the best of the authors’ knowledge, the stability analysis
of momentum-based control strategies in the contexts of
floating base systems is still missing. The contribution of this
paper goes along this direction by considering a humanoid
robot standing on one foot, and is then twofold. First, we
arXiv:1603.04178v4  [math.OC]  16 Jul 2017
present numerical evidence that classical momentum-based
control strategies may lead to unstable zero dynamics, thus
meaning that classical postural tasks are not sufficient in
these cases. The main cause of this instability is the lack
of orientation correction terms at the angular momentum
level: we show that correction terms of the form of angular
momentum integrals are sufficient for ensuring asymptotic
stability of the closed loop system, which is proved by means
of a Lyapunov analysis. The postural control, however,
is modified with respect to (w.r.t.) state-of-the-art choices.
The validity of the presented controller is tested both in
simulation and on the humanoid robot iCub.
This paper is organized as follows. Section II intro-
duces the notation, the system modeling, and also recalls
a classical momentum-based control strategy. Section III
presents numerical results showing that momentum based
control strategies for humanoid robots may lead to unstable
zero dynamics. Section IV presents a modification of the
momentum based control strategy for which stability and
convergence can be proven. Section V discusses the numer-
ical and experimental validation of the proposed approach.
Conclusions and perspectives conclude the paper.
II. BACKGROUND
A. Notation
• I denotes an inertial frame, with its z axis pointing
against the gravity. The constant g denotes the norm of
the gravitational acceleration.
• ei ∈Rm is the canonical vector, consisting of all zeros
but the i-th component that is equal to one.
• Given two orientation frames A and B, and two vectors
Ap, Bp ∈R3 expressed in these orientation frames, the
rotation matrix ARB is such that Ap = ARB Bp.
• Let S(x) ∈R3×3 be the skew-symmetric matrix such
that S(x)y = x × y, where × is the cross product
operator in R3.
• Given a function f(x, y) : Rn × Rm →Rp, the partial
derivative of f(·) w.r.t. the variable x is denoted as
∂xf(x, y) = ∂f(x,y)
∂x
∈Rp×n.
B. Modelling
It is assumed that the robot is composed of n + 1 rigid
bodies, called links, connected by n joints with one degree
of freedom each. We also assume that the multi-body system
is free floating, i.e. none of the links has an a priori
constant pose with respect to the inertial frame. The robot
configuration space can then be characterized by the position
and the orientation of a frame attached to a robot’s link,
called base frame B, and the joint configurations. Thus, the
configuration space is defined by Q = R3 × SO(3) × Rn.
An element of Q is then a triplet q = (IpB, IRB, qj), where
(IpB, IRB) denotes the origin and orientation of the base
frame expressed in the inertial frame, and qj denotes the joint
angles. It is possible to define an operation associated with
the set Q such that this set is a group. Given two elements q
and ρ of the configuration space, the set Q is a group under
the following operation: q · ρ = (pq + pρ, RqRρ, qj + ρj).
Furthermore, one easily shows that Q is a Lie group. Then,
the velocity of the multi-body system can be characterized
by the algebra V of Q defined by: V = R3 × R3 × Rn.
An element of V is then a triplet ν = (I ˙pB,I ωB, ˙qj) =
(vB, ˙qj), where IωB is the angular velocity of the base frame
expressed w.r.t. the inertial frame, i.e. I ˙RB = S(IωB)IRB.
We also assume that the robot is interacting with the envi-
ronment exchanging nc distinct wrenches1. The application
of the Euler-Poincar´e formalism [21, Ch. 13.5] to the multi-
body system yields the following equations of motion:
M(q) ˙ν + C(q, ν)ν + G(q) = Bτ +
nc
X
k=1
J⊤
Ckfk
(1)
where M ∈Rn+6×n+6 is the mass matrix, C ∈Rn+6×n+6
is the Coriolis matrix, G ∈Rn+6 is the gravity term,
B = (0n×6, 1n)⊤is a selector matrix, τ ∈Rn is a vector
representing the actuation joint torques, and fk ∈R6 denotes
the k-th external wrench applied by the environment on the
robot. We assume that the application point of the external
wrench is associated with a frame Ck, attached to the link
on which the wrench acts, and has its z axis pointing in
the direction of the normal of the contact plane. Then, the
external wrench fk is expressed in a frame whose orientation
is that of the inertial frame I, and whose origin is that
of Ck, i.e. the application point of the external wrench fk.
The Jacobian JCk = JCk(q) is the map between the robot’s
velocity ν and the linear and angular velocity
IvCk :=
(I ˙pCk,I ωCk) of the frame Ck, i.e. IvCk = JCk(q)ν. The
Jacobian has the following structure:
JCk(q) =
h
Jb
Ck(q)
Jj
Ck(q)
i
∈R6×n+6, (2a)
Jb
Ck(q) =
"
13
−S(IpCk −IpB)
03×3
13
#
∈R6×6.
(2b)
Lastly, it is assumed that holonomic constraints act on
System (1). These constraints are of the form c(q) = 0,
and may represent, for instance, a frame having a constant
pose w.r.t. the inertial frame. In the case where this frame
corresponds to the location at which a contact occurs on a
link, we represent the holonomic constraint as JCk(q)ν=0.
C. Block-Diagonalization of the Mass Matrix
This section recalls a new expression of the equations of
motion (1). In particular, the next lemma presents a change
of coordinates in the state space (q, ν) that transforms the
system dynamics (1) into a new form where the mass matrix
is block diagonal, thus decoupling joint and base frame
accelerations. The obtained equations of motion are then
used in the remaining of the paper.
Lemma 1. The proof is given in [22]. Consider the equations
of motion given by (1) and the mass matrix partitioned as
following
M =
"
Mb
Mbj
M ⊤
bj
Mj
#
1As an abuse of notation, we define as wrench a quantity that is not the
dual of a twist
with Mb ∈R6×6, Mbj ∈R6×n and Mj ∈Rn×n. Perform
the following change of state variables:
q := q,
¯ν := T(q)ν,
(3)
with
T :=
"
cXB
cXBM −1
b
Mbj
0n×6
1n
#
,
(4a)
cXB :=
"
13
−S(Ipc −IpB)
03×3
13
#
(4b)
where the superscript c denotes the frame with the origin
located at the center of mass, and with orientation of I.
Then, the equations of motion with state variables (q, ν) can
be written in the following form
M(q)˙ν + C(q, ν)ν + G = Bτ +
nc
X
i=1
J⊤
Cifi,
(5)
with
M(q) = T −⊤MT −1 =
"
M b(q)
06×n
0n×6
M j(qj)
#
,
(6a)
C(q, ν) = T −⊤(M ˙T −1 + CT −1),
(6b)
G = T −⊤G = mge3,
(6c)
JCi(q) = JCi(q)T −1 =
h
Jb
Ci(q)
Jj
Ci(qj)
i
,
(6d)
M b(q) =
m13
03×3
03×3
I(q)

, Jb
Ci(q)=
"
13
−S(pCi−Ipc)
03×3
13
#
where m is the mass of the robot and I is the inertia
matrix computed with respect to the center of mass, with
the orientation of I.
The above lemma points out that the mass matrix of the
transformed system (5) is block diagonal, i.e. the transformed
base acceleration is independent from the joint acceleration.
More precisely, the transformed robot’s velocity ν is given
by ν =

I ˙p⊤
c
Iω⊤
c
˙q⊤
j
⊤
where I ˙pc is the velocity of the
center-of-mass of the robot, and Iωc is the so-called average
angular velocity2[24],[25],[26]. Hence, Eq. (5) unifies what
the specialized robotic literature usually presents with two
sets of equations: the equations of motion of the free floating
system and the centroidal dynamics3 when expressed in
terms of the average angular velocity. For the sake of
correctness, let us remark that defining the average angular
velocity as the angular velocity of the multi-body system is
not theoretically sound. In fact, the existence of a rotation
matrix R(q) ∈SO(3) such that ˙R(q)R⊤(q) = S(Iωc), i.e.
the integrability of Iωc, is still an open issue.
Observe also that the gravity term G is constant and
influences the acceleration of the center-of-mass only. This
2The term Iωc is also known as the locked angular velocity [23].
3In the specialized literature, the terms centroidal dynamics are used to
indicate the rate of change of the robot’s momentum expressed at the center-
of-mass, which then equals the summation of all external wrenches acting
on the multi-body system [26].
is a direct consequence of (3)-(4) and of the property that
G(q) = Mge3, with e3 ∈Rn+6.
Remark 1. In the sequel, we assume that the equations
of motion are given by
(5), i.e. the mass matrix is block
diagonal. As an abuse of notation but for the sake of
simplicity, we hereafter drop the overline notation.
D. A classical momentum-based control strategy
This section recalls a classical momentum-based control
strategy when implemented as a two-layer stack-of-task.
We assume that the objective is the control of the robot
momentum and the stability of the zero dynamics.
Recall that the configuration space of the robot evolves in
a group of dimension4 n + 6. Hence, besides pathological
cases, when the system is subject to a set of holonomic
constraints of dimension k, the configuration space shrinks
into a space of dimension n + 6 −k. The stability analysis
of the constrained system may then require to determine the
minimum set of coordinates that characterize the evolution
of the constrained system. This operation is, in general, far
from obvious because of the topology of the group Q.
Now, in the case the holonomic constraint is of the form
T(q) = constant, with T(q) ∈R3 × SO(3), i.e. a robot
link has a constant position-and-orientation w.r.t. the inertial
frame, one gets rid of the topology related problems of Q by
relating the base frame B and the constrained frame. In this
case, the minimum set of coordinates belongs to Rn and can
be chosen as the joint variables qj. In light of the above, we
make the following assumption.
Assumption 1. Only one frame associated with a robot link
has a constant position-and-orientation with respect to the
inertial frame.
Without loss of generality, it is assumed that the only
constrained frame is that between the environment and one
of the robot’s feet. Consequently, one has:
nc
X
k=1
J⊤
Ckfk = J⊤(q)f,
(7)
where J(q) ∈R6×n+6 is the Jacobian of a frame attached to
the foot’s sole in contact with the environment, and f ∈R6
the contact wrench. Differentiating the kinematic constraint
J(q)ν =

Jb
Jj

ν = 0
(8)
associated with the contact, yield
Jb
Jj
 ˙vB
¨qj

+
h
˙Jb
˙Jj
i 
vB
˙qj

= 0.
(9)
1) Momentum control: Thanks to the results presented in
Lemma 1, the robot’s momentum H ∈R6 is given by H =
MbvB. The rate-of-change of the robot momentum equals
the net external wrench acting on the robot, which in the
present case reduces to the contact wrench f plus the gravity
wrench. To control the robot momentum, it is assumed that
4With group dimension we here mean the dimension of the associated
algebra V.
the contact wrench f can be chosen at will. Note that given
the particular form of (5), the first six rows correspond to
the dynamics of the robot’s momentum, i.e.
d
dt(MbvB) = J⊤
b f −mge3 = ˙H(f)
(10)
where H := (HL, Hω), with HL, Hω ∈R3 linear and
angular momentum, respectively. The control objective can
then be defined as the stabilization of a desired robot
momentum Hd. Let us define the momentum error as follows
˜H = H −Hd. The control input f in Eq. (10) is chosen so
as
˙H(f) = ˙H∗:= ˙Hd −Kp ˜H −KiI ˜
H
(11a)
˙I ˜
H = ˜H
(11b)
where Kp, Ki ∈R6×6 are two symmetric, positive definite
matrices. It is important to note that a classical choice for
the matrix Ki consists in [11],[27]:
Ki =
 
KL
i
03×3
03×3
03×3
!
,
(12)
i.e. the integral correction term at the angular momentum
level is equal to zero, while the positive definite matrix
KL
i
∈
R3×3 is used for tuning the tracking of a desired
center-of-mass position when the initial conditions of the
integral in (11) are properly set.
Assumption 1 implies that the contact wrench satisfying
Eq. (11) can be chosen as
f = J−⊤
b

˙H∗+ mge3

.
(13)
Now, to determine the control torques that instantaneously
realize the contact force given by (13), we use the dynamic
equations (5) along with the constraints (9), which yield
τ = Λ†(JM −1(h −J⊤f) −˙Jν) + NΛτ0
(14)
where Λ = JjM −1
j
∈R6×n, NΛ ∈Rn×n is the nullspace
projector of Λ, h ∈Rn+6 is the vector containing both the
Coriolis and gravity terms and τ0 ∈Rn is a free variable.
2) Stability of the zero dynamics: The stability of the zero
dynamics is usually attempted by means of a so called “pos-
tural task”, which exploits the free variable τ0. A classical
state-of-the-art choice for this postural task consists in: τ0 =
hj−J⊤
j f−Kj
p(qj−qd
j )−Kj
d ˙qj, where hj−J⊤
j f compensates
for the nonlinear effect and the external wrenches acting
on the joint space of the system. Hence, the (desired) input
torques that are in charge of stabilizing both a desired robot
momentum Hd and the associated zero dynamics are given
by
τ = Λ†(JM −1(h −J⊤f) −˙Jν) + NΛτ0
(15a)
f = J−⊤
b

˙H∗+ mge3

(15b)
τ0 = hj −J⊤
j f −Kj
p(qj −qd
j ) −Kj
d ˙qj
(15c)
We present below numerical results showing that (15) with
Ki as (12) may lead to unstable zero dynamics.
III. NUMERICAL EVIDENCE OF UNSTABLE ZERO
DYNAMICS
A. Simulation Environments
Two different simulation setups have been exploited to
perform the numerical validation. In both cases, we simulate
the humanoid robot iCub with 23 DoFs [28].
1) Custom setup: It is in charge of integrating the dynam-
ics (5) when it is subject to the constraint (9). We parametrize
SO(3) by means of a quaternion representation Q ∈R4.
The resulting state space system, which is integrated through
time, is then: χ := (pB, Q, qj, ˙pB, ωB, ˙qj), and its derivative
is given by ˙χ = ( ˙pB, ˙Q, ˙qj, ˙ν)
The constraints (9), as well as |Q| = 1, are then enforced
during the integration phase, and additional correction terms
have been added [29]. The system evolution is then obtained
by integrating the constrained dynamical system with the
numerical integrator MATLAB ode15s.
2) Gazebo setup: The Gazebo simulator [30] is the other
simulation setup used for our tests. Of the different physic
engines that can be used with Gazebo, we chose the Open
Dynamics Engine (ODE). Differently from the previous sim-
ulation environment, Gazebo allows one for more flexibility.
Indeed, we only have to specify the model of the robot, and
the constraints arise naturally while simulating. Furthermore,
Gazebo integrates the dynamics with a fixed step semi-
implicit Euler integration scheme. Another advantage of
using Gazebo w.r.t. the custom integration scheme previously
presented consists in the ability to test in simulation the same
control software used on the real robot.
B. Unstable Zero Dynamics
To show that the momentum-based control strategies may
lead to unstable zero dynamics, we control the linear mo-
mentum of the robot so as to follow a desired center of mass
trajectory, i.e. a sinusoidal reference along the y coordinate
with amplitude 0.05m and frequency 0.3Hz. The reference
qd
j is set equal to its joint initial value, i.e. qd
j = qj(0).
1) Tests on the robot balancing on one foot in the custom
simulation setup: we present simulation results obtained by
applying the control laws (15) with Ki as in (12). It is
assumed that the left foot is attached to ground, and no other
external wrench applies to the robot.
Figure 1 shows typical simulation results of the conver-
gence to zero of the robot’s momentum error, thus meaning
that the wrench (13) ensures the stabilization of ˜H towards
zero. Figure 2, instead, depicts the joint position error norm
|qj −qd
j |. This figure shows that the norm of the joint angles
increases while the robot’s momentum is kept equal to zero,
which is a classical behavior of unstable zero dynamics.
2) Tests when the robot balances on two feet in the Gazebo
simulation setup: Tests on the stability of the zero dynamics
have been carried out also in the case the robot stands on
both feet. The main difference between the control algorithm
running in this case and that in (15) resides in the choice of
contact forces and in the holonomic constraints acting on the
system. More precisely, the contact wrench f is now a twelve
dimensional vector, and composed of the contact wrenches
fL, fR ∈R6 between the floor and left and right feet,
respectively. Hence, f =
fL, fR

∈R12. Also, let JL, JR ∈
R6×n+6 denote the Jacobian of two frames associated with
the contact locations of the left and right foot, respectively.
Then, J =
h
J⊤
L , J⊤
R
i⊤
∈R12×n+6. By assuming that the
contact wrenches can still be used as virtual control input in
the dynamics of the robot’s momentum ˙H in (10), one is left
with a six-dimensional redundancy of the contact wrenches
to impose H(f) = ˙H∗. We use this redundancy to minimize
the joint torques. In the language of Optimization Theory,
the above control objectives can be formulated as follows.
f ∗= argmin
f
|τ ∗(f)|
(16a)
s.t.
Cf < b
(16b)
˙H(f) = ˙H∗
(16c)
τ ∗(f) = argmin
τ
|τ(f) −τ0(f)|
(17)
s.t.
˙J(q, ν)ν + J(q) ˙ν = 0
(18a)
˙ν = M −1(Bτ + J⊤(q)f−h(q, ν))
(18b)
τ0 = hj −J⊤
j f−Kj
p(qj −qd
j )−Kj
d ˙qj (18c)
Note that the additional constraint (16b) ensures that the
desired contact wrenches f belong to the associated friction
cones. Once the optimum f ∗has been determined, the input
torques τ are obtained by evaluating the expression (17), i.e.
τ = τ ∗(f ∗)
(19)
Figure 3 depicts a typical behavior of the joint position
error norm |qj −qd
j | when the above control algorithm is
applied. It is clear from this figure that the instability of the
zero dynamics is observed also in the case where the robot
stands on two feet.
IV. CONTROL DESIGN
To circumvent the problems related to the stability of the
zero dynamics discussed in the previous section, we propose
a modification of the control laws (15) that allows us to show
stable zero dynamics of the constrained, closed loop sys-
tem. The following results exploit the so-called centroidal-
momentum-matrix JG(q) ∈R6×n+6, namely the mapping
between the robot velocity ν and the robot’s momentum H:
H = JG(q)ν. Now, observe that thanks to the results of
Lemma 1 one has JG(q) =
Mb
06×n

. Then, observe
that when Assumption 1 is satisfied, Eq. (8) allows us to
write the robot’s momentum linearly w.r.t. the robot’s joint
velocity, i.e. H = ¯JG(qj) ˙qj, where ¯JG(qj) ∈R6×n is:
¯JG(qj) :=
"
JL
G(qj)
Jω
G(qj)
#
= −MbJ−1
b
Jj,
(20)
and JL
G(qj), Jω
G(qj) ∈R3×n. In light of the above, the
following result holds.
0
10
20
30
40
50
60
time [s]
0
0.2
0.4
0.6
0.8
1
j ~Hj
Fig. 1.
Time evolution of the robot’s momentum error when standing on
one foot and when the control law (15) is applied. Simulation run with the
custom environment.
0
10
20
30
40
50
60
time [s]
0
0.2
0.4
0.6
0.8
1
jqj ! qd
j j [rad]
Fig. 2.
Time evolution of the norm of the position error |qj −qd
j | when
the robot is standing on one foot and when the control law (15) is applied.
Simulation run with the custom environment.
0
10
20
30
40
50
60
time [s]
0
0.2
0.4
0.6
0.8
1
jqj ! qd
j j [rad]
Fig. 3.
Time evolution of the norm of the position error |qj −qd
j | when
the robot is standing on two feet and when the control law (16)- (19) is
applied. Simulation run with the Gazebo environment.
Lemma 2. Assume that Assumption 1 holds, and that the
robot possesses more than six degrees of freedom, i.e. n ≥6.
In addition, assume also that Hd = 0. Let
(qj, ˙qj) = (qd
j , 0)
(21)
denote the equilibrium point associated with the constrained,
closed loop system and assume that the matrix Λ
=
Jj(q)M −1
j
(q) is full row rank in a neighborhood of (21).
Apply the control laws (15) with
˙I ˜
H =
"
JL
G(qj)
Jω
G(qd
j )
#
˙qj
(22)
Ki > 0,
Kj
p = Kj
pNΛMj,
Kj
d = Kj
dNΛMj, (23)
where Kj
p, Kj
d ∈Rn×n are two constant, positive definite
matrices. Then, the equilibrium point (21) of the constrained,
closed loop dynamics is asymptotically stable.
The proof is in Appendix. Lemma 2 shows that the asymp-
totic stability of the equilibrium point (21) of the constrained,
closed loop dynamics can be ensured by modifying the
integral correction terms, and by modifying the gains of the
postural task. As a consequence, the asymptotic stability of
the equilibrium point (21) implies that the zero dynamics are
locally asymptotically stable.
The fact that the gain matrix Ki must be positive defi-
nite conveys the necessity of closing the control loop with
orientation terms at the angular momentum level. In fact,
some authors intuitively close the angular momentum loop
by using the orientation of the robot’s torso [7].
The proof of Lemma 2 exploits the fact that the minimum
coordinates of the robot configuration space when Assump-
tion 1 holds is given by the joint angles qj. The analysis
focuses on the closed loop dynamics of the form ¨qj =
f(qj, ˙qj) which is then linearized around the equilibrium
point (21). By means of a Lyapunov analysis, one shows
that the equilibrium point is asymptotically stable. One of
the main technical difficulties when linearizing the equation
¨qj = f(qj, ˙qj) comes from the fact that the closed loop
dynamics depends on the integral of the robot momentum,
i.e.
I ˜
H(t) = I ˜
H(0) +
Z t
0
"
JL
G(qj(s))
Jω
G(qd
j )
#
˙qj(s) ds
(24)
The partial derivative of I ˜
H(t) w.r.t. the state (qj, ˙qj) is,
in general, not obvious because the matrix JL
G(qj) may not
be integrable. Let us observe, however, that the first three
rows of ˙I ˜
H(t) correspond to the velocity of the center-of-
mass times the robot’s mass when expressed in terms of the
minimal coordinates qj, i.e. JL
G(qj) ˙qj = m ˙xc, with ˙xc ∈R3
the velocity of the robot’s center-of-mass. Clearly, this means
that JL
G(qj) is integrable, and that
∂qjI ˜
H =
"
JL
G(qj)
Jω
G(qd
j )
#
∂˙qjI ˜
H = 0
(25)
Remark 2. Lemma 2 suggests that applying the control
laws (15) with the control gains as (23) can still guarantee
stability and convergence of the equilibrium point. First,
observe that the main difference between the variable I ˜
H
governed by the two expressions (11b) and (22) resides only
in the last three equations. Then, more importantly, note that
the momentum H when Assumption 1 holds can be expressed
as follows H = ¯JG(qd
j ) ˙qj +o(qj −qd
j , ˙qj) which implies that
Z t
0
H ds = ¯JG(qd
j )(qj −qd
j ) +
Z t
0
o(qj −qd
j , ˙qj) ds
As a consequence, the linear approximations of the inte-
grals I ˜
H governed by (11b) and (22) coincide when
lim
(qj−qd
j , ˙qj)→0
|
R t
0 o(qj −qd
j , ˙qj) ds|
|(qj−qd
j , ˙qj)|
= 0
(26)
Under the above assumption (26), the linear approximation
of the control laws (15) when evaluated with (11b) and (22)
coincide, and stability and convergence of the equilibrium
point (qd
j , 0) can still be proven.
V. SIMULATIONS AND EXPERIMENTAL RESULTS
This section shows simulation and experimental results
obtained by applying the control laws (15)-(23) and (16)-
(19)-(23). To show the improvements of the control modifi-
cation in Lemma 2, we apply the same reference signal of
Section III, which revealed unstable zero dynamics. Hence,
the desired linear momentum is chosen so as to follow a
sinusoidal reference on the center of mass. Also, control
gains are kept equal to those used for the simulations
presented in Section III.
A. Simulation results
Figures 4 – 5 show the norm of the joint errors |qj −qd
j |
when the robot stands on either one or two feet, respectively.
Experiments on two feet have been performed to verify the
robustness of the new control architecture. The simulations
are performed both with the custom and Gazebo environ-
ment. As expected, the zero dynamics is now stable, and no
divergent behavior of the robot joints is observed.
B. Results on the iCub Humanoid Robot
We then went one step further and implemented the
control algorithm (16)-(19) with the modification presented
in Lemma 2 on the real humanoid robot. The robotic platform
used for testing is the iCub humanoid robot [28]. For the
purpose of the proposed control law, iCub is endowed with
23 degrees of freedom. A low level torque control loop,
running at 1kHz, is responsible for stabilizing any desired
torque reference signal.
Figures 6 – 7 show the joint position error |qj −qd
j | and
the center of mass error. Though the center of mass does not
converge to the desired value, all signals are bounded, and the
control modification presented in Lemma 2 does not pose any
barrier for the implementation of the control algorithm (16)-
(19) on a real platform.
VI. CONCLUSIONS AND FUTURE WORKS
Momentum-based controllers are an efficient control strat-
egy for performing balancing and walking tasks on hu-
manoids. In this paper, we presented numerical evidence
that a stack-of-task approach to this kind of controllers
may lead to an instability of the zero dynamics. In partic-
ular, to ensure stability, it is necessary to close the loop
with orientation terms at the momentum level. We show
a modification of state-of-the-art momentum based control
strategies that ensure asymptotic stability, which was shown
by performing Lyapunov analysis on the linearization of the
closed loop system around the equilibrium point. Simulation
and experimental tests validate the presented analysis.
The stack-of-task approach strongly resembles a cascade
of dynamical systems. It is the authors’ opinion that the
stability of the whole system can be proved by using the
general framework of stability of interconnected systems. A
critical point needed to prove the stability of constrained
dynamical system is to define the minimum set of coordinates
identifying its evolution. This can be straightforward in case
of one contact, but the extension to multiple contacts is not
trivial and must be considered carefully.
0
10
20
30
40
50
60
time [s]
0
0.2
0.4
0.6
0.8
1
jqj ! qd
j j [rad]
Fig. 4.
Time evolution of the norm of the position error |qj −qd
j | when
the robot is standing on one foot and when the control law (15)–(23)–(22)
is applied. Simulation run with the custom environment.
0
10
20
30
40
50
60
time [s]
0
0.2
0.4
0.6
0.8
1
jqj ! qd
j j [rad]
Fig. 5. Time evolution of the norm of the position error |qj −qd
j | when the
robot is standing on two feet and when the control law (16)–(19)–(23)–(22)
is applied. Simulation run with the Gazebo environment.
APPENDIX: PROOF OF LEMMA 2
As described in Section IV the proof is composed of
two steps. First, we linearize the constrained closed loop
dynamics around the equilibrium (qd
j , 0). Then, by means of
Lyapunov analysis, we show that the equilibrium point is
asymptotically stable.
1) Linearization: Consider that Assumption 1 holds, and
that we apply the control laws (15)-(22) with the gains
as (23). The closed loop joint space dynamics of system (5)
constrained by (9) is given by the following equation:
Mj ¨qj + hj −J⊤
j f = τ
(27)
Now, rewrite (15) as follows: τ = Λ+(Λ(hj −J⊤
j f) +
JbM −1
b
(hb −J⊤
b f) −˙Jν) + NΛτ0. Therefore, Eq. (27) can
be simplified into:
¨qj = M −1
j
(Λ+(JbM −1
b
(hb −J⊤
b f) −˙Jν) −NΛu0) (28)
where u0 = Kj
p(qj −qd
j ) + Kj
d ˙qj, while hb = Cbνb +
Cbj ˙qj +mge3, and Cb ∈R6×6, Cbj ∈R6×n, Cjb ∈Rn×6,
Cj ∈Rn×n are obtained from the following partition of the
Coriolis matrix:
C =
 Cb
Cbj
Cjb
Cj

Substituting (13) into (28) and grouping together the terms
that are linear with respect of joint velocity yield:
¨qj = −M −1
j
h
Λ†(JbM −1
b
˙H∗+ Γ ˙qj) + NΛu0
i
(29)
where Γ = ˙Jj −JbM −1
b
Cbj + (JbM −1
b
Cb −˙Jb)J−1
b
Jj.
Define
the
state
x
as
x
:=
h
x⊤
1
x⊤
2
i⊤
=
0
10
20
30
40
50
60
time [s]
0
0.2
0.4
0.6
0.8
1
jqj ! qd
j j [rad]
Fig. 6.
Time evolution of the position error norm |qj −qd
j | when the
robot is standing on two feet and when the laws (16)–(19)–(23)–(22) are
applied. Experiment run on the humanoid robot iCub.
0
10
20
30
40
50
60
time [s]
-5
-2.5
0
2.5
5
~xG [cm]
~xGx
~xGy
~xGz
Fig. 7.
Time evolution of robot center-of-mass errors ˜xG when the robot
stands on two feet and when the laws (16)–(19)–(23)–(22) are applied.
Experiment run on the humanoid robot iCub.
h
q⊤
j −qd⊤
j
˙q⊤
j
i⊤
. Since Hd ≡0, the linearized dynamical
system about the equilibrium point (qd
j , 0) is given by
˙x =
∂qj ˙x1
∂˙qj ˙x1
∂qj ˙x2
∂˙qj ˙x2

x =
0n×n
1n
A1
A2

x
(30)
To find the matrices A1, A2 ∈Rn×n, one has to evaluate the
following partial derivatives
∂y¨qj = −
6
X
i=1
∂y(M −1
j
Λ†JbM −1
b
ei)e⊤
i ˙H∗−M −1
j
NΛ∂yu0
−
n
X
i=1
∂y(M −1
j
NΛei)e⊤
i u0 −M −1
j
Λ†JbM −1
b
∂y ˙H∗
with y = {qj, ˙qj}. Note that ˙H∗= 0 and u0 = 0 when eval-
uated at qj = qd
j and ˙qj = 0. We thus have to compute only
the partial derivatives of ˙H∗and u0. The latter is trivially
given by ∂qju0 = Kj
pNΛMj and ∂˙qju0 = Kj
dNΛMj. The
former can be calculated via Eq. (25). In light of the above
we obtain the expressions of the matrices in (30):
A1 = −M −1
j
Λ†JbM −1
b
KiMbJ−1
b
Jj −M −1
j
NΛKj
pNΛMj
A2 = −M −1
j
Λ†JbM −1
b
KpMbJ−1
b
Jj −M −1
j
NΛKj
dNΛMj.
2) Proof of Asymptotic Stability: Consider now the fol-
lowing Lyapunov candidate:
V (x)=1
2
h
x⊤
1 M ⊤
j Q1Mjx1+x⊤
2 M ⊤
j Q2Mjx2
i
where Mj = Mj(qd
j ), and
Q1 := Λ⊤J−⊤
b
M ⊤
b KiMbJ−1
b
Λ + NΛKj
pNΛ
Q2 := Λ⊤J−⊤
b
M ⊤
b MbJ−1
b
Λ + NΛ
calculated at x1 = 0, x2 = 0. V is a properly defined
candidate, in fact V = 0 ⇐⇒x = 0 and is positive definite
otherwise. Indeed, Q1 can be rewritten in the following way:
Q1 =
h
Λ⊤
NΛ
i "
J−⊤
b
M ⊤
b KiMbJ−1
b
0
0
Kj
p
#  Λ
NΛ

.
and, because Λ and NΛ are orthogonal Q1 is positive definite.
The same reasoning can be applied to Q2.
We can now consider the time derivative of V :
˙V = x⊤
1 M ⊤
j Q1Mjx2 + x⊤
2 M ⊤
j Q2Mj ˙x2
= −x⊤
2 M ⊤
j (Λ⊤J−⊤
b
M ⊤
b KpMbJ−1
b
Λ
+NΛKj
dNΛ)Mjx2 ≤0.
The stability of the equilibrium point x = 0 associated with
the linear system (30) thus follows. To prove the asymptotic
stability of the equilibrium point x = 0, which implies its
asymptotic stability when associated with the nonlinear sys-
tem (29), we have to resort to LaSalle’s invariance principle.
Let us define the invariant set S := {x :
˙V (x) = 0}
that implies S = {(x1, 0)}. It is easy to verify that the
only trajectory starting in S and remains in S is given by
x1 = 0 thus proving LaSalle’s principle. As a consequence,
the equilibrium point x = 0 ⇒(qj, ˙qj) = (qd
j , 0) is
asymptotically stable.
REFERENCES
[1] S. Caux, E. Mateo, and R. Zapata, “Balance of biped robots: special
double-inverted pendulum,” Systems, Man, and Cybernetics, 1998.
1998 IEEE International Conference on, 1998.
[2] K. Hirai, M. Hirose, Y. Haikawa, and T. Takenaka, “The development
of Honda humanoid robot,” Robotics and Automation, 1998. Proceed-
ings. 1998 IEEE International Conference on, 1998.
[3] S. Hyon, J. Hale, and G. Cheng, “Full-body compliant human-
humanoid interaction: Balancing in the presence of unknown external
forces,” Robotics, IEEE Transactions on, Oct 2007.
[4] Q. Huang, K. Kaneko, K. Yokoi, S. Kajita, T. Kotoku, N. Koyachi,
H. Arai, N. Imamura, K. Komoriya, and K. Tanie, “Balance control
of a biped robot combining off-line pattern with real-time modifica-
tion,” Robotics and Automation, 2000. Proceedings. ICRA ’00. IEEE
International Conference on, 2000.
[5] R. Featherstone, Rigid Body Dynamics Algorithms.
Secaucus, NJ,
USA: Springer-Verlag New York, Inc., 2007.
[6] J. A. Acosta and M. Lopez-Martinez, “Constructive feedback lineariza-
tion of underactuated mechanical systems with 2-DOF,” Decision and
Control, 2005 and 2005 European Control Conference. CDC-ECC ’05.
44th IEEE Conference on, 2005.
[7] C. Ott, M. Roa, and G. Hirzinger, “Posture and balance control
for biped robots based on contact force optimization,” in Humanoid
Robots (Humanoids), 2011 11th IEEE-RAS International Conference
on, Oct 2011, pp. 26–33.
[8] P. Wensing and D. Orin, “Generation of dynamic humanoid behaviors
through task-space control with conic optimization,” in Robotics and
Automation (ICRA), 2013 IEEE International Conference on, May
2013, pp. 3103–3109.
[9] F. Nori, S. Traversaro, J. Eljaik, F. Romano, A. Del Prete, and
D. Pucci, “iCub whole-body control through force regulation on rigid
noncoplanar contacts,” Frontiers in Robotics and AI, vol. 2, no. 6,
2015.
[10] B. Stephens and C. Atkeson, “Dynamic balance force control for
compliant humanoid robots,” in Intelligent Robots and Systems (IROS),
2010 IEEE/RSJ International Conference on, Oct 2010, pp. 1248–
1255.
[11] A. Herzog, L. Righetti, F. Grimminger, P. Pastor, and S. Schaal, “Bal-
ancing experiments on a torque-controlled humanoid with hierarchical
inverse dynamics,” in Intelligent Robots and Systems (IROS 2014),
2014 IEEE/RSJ International Conference on, Sept 2014, pp. 981–988.
[12] T. Koolen, S. Bertrand, G. Thomas, T. de Boer, T. Wu, J. Smith,
J. Englsberger, and J. Pratt, “Design of a momentum-based control
framework and application to the humanoid robot atlas,” International
Journal of Humanoid Robotics, vol. 13, p. 34, March 2016.
[13] L. Righetti, J. Buchli, M. Mistry, and S. Schaal, “Inverse dynamics
control of floating-base robots with external constraints: A unified
view,” IEEE International Conference on Robotics and Automation,
May 2011.
[14] ——, “Control of legged robots with optimal distribution of contact
forces,” in Humanoid Robots (Humanoids), 2011 11th IEEE-RAS
International Conference on, Oct 2011, pp. 318–324.
[15] J. Nakanish, M. Mistry, and S. Schaal, “Inverse Dynamics Control
with Floating Base and Constraints,” Robotics and Automation, 2007
IEEE International Conference on, pp. 1942 – 1947, 2007.
[16] M. Hopkins, R. Griffin, A. Leonessa, B. Lattimer, and T. Furukawa,
“Design of a compliant bipedal walking controller for the darpa
robotics challenge,” in Humanoid Robots (Humanoids), 2015 IEEE-
RAS 15th International Conference on, Nov 2015, pp. 831–837.
[17] P.-B. Wieber, Holonomy and Nonholonomy in the Dynamics of
Articulated Motion.
Berlin, Heidelberg: Springer Berlin Heidelberg,
2006, pp. 411–425. [Online]. Available: http://dx.doi.org/10.1007/
978-3-540-36119-0 20
[18] M. Liu and V. Padois, “Reactive whole-body control for humanoid
balancing on non-rigid unilateral contacts,” Intelligent Robots and
Systems (IROS), 2015 IEEE/RSJ International Conference on, pp. 3981
– 3987, 2015.
[19] A. Hofmann, M. Popovic, and H. Herr, “Exploiting angular momentum
to enhance bipedal center-of-mass control,” Robotics and Automation.
ICRA ’09. IEEE International Conference on. 2009, 2009.
[20] H. Herr and M. Popovic, “Angular momentum in human walking,”
Journal of Experimental Biology, vol. 211, no. 4, pp. 467–481, 2008.
[21] J. E. Marsden and T. S. Ratiu, Introduction to Mechanics and Symme-
try: A Basic Exposition of Classical Mechanical Systems.
Springer
Publishing Company, Incorporated, 2010.
[22] S. Traversaro, D. Pucci, and F. Nori, “On the base frame choice
in free-floating mechanical systems and its connection to centroidal
dynamics,” Submitted to Humanoid Robots (Humanoids), 2016
IEEE-RAS International Conference on, 2016. [Online]. Available:
https://traversaro.github.io/preprints/changebase.pdf
[23] J. E. Marsden and J. Scheurle, “The reduced euler-lagrange equations,”
Fields Institute Comm, vol. 1, pp. 139–164, 1993.
[24] J. Jellinek and D. Li, “Separation of the energy of overall rotation in
any n-body system,” Physical review letters, vol. 62, no. 3, p. 241,
1989.
[25] H. Ess´en, “Average angular velocity,” European journal of physics,
vol. 14, no. 5, p. 201, 1993.
[26] D. Orin, A. Goswami, and S.-H. Lee, “Centroidal dynamics of a
humanoid robot,” Autonomous Robots, 2013.
[27] S.-H. Lee and A. Goswami, “Ground reaction force control at each
foot: A momentum-based humanoid balance controller for non-level
and non-stationary ground,” in Intelligent Robots and Systems (IROS),
2010 IEEE/RSJ International Conference on, Oct 2010, pp. 3157–
3162.
[28] G. Metta, L. Natale, F. Nori, G. Sandini, D. Vernon, L. Fadiga, C. von
Hofsten, K. Rosander, M. Lopes, J. Santos-Victor, A. Bernardino, and
L. Montesano, “The iCub humanoid robot: An open-systems platform
for research in cognitive development,” Neural Networks, vol. 23, no.
89, pp. 1125 – 1134, 2010, social Cognition: From Babies to Robots.
[29] S. Gros, M. Zanon, and M. Diehl, “Baumgarte stabilisation over the
SO(3) rotation group for control,” 2015 54th IEEE Conference on
Decision and Control (CDC), pp. 620–625, December 2015.
[30] N. Koenig and A. Howard, “Design and use paradigms for gazebo,
an open-source multi-robot simulator,” Intelligent Robots and Systems,
2004. (IROS 2004). Proceedings. 2004 IEEE/RSJ International Con-
ference on, pp. 2149 – 2154, 2004.