IEEE Robotics & Automation Magazine - December 2015 - 18

the key to safe behavior. For example, balance will always be
considered a top-priority objective, while other objectives,
such as visibility and posture, are accomplished only if possible but without disturbing the balance (or the possible manipulation or locomotion tasks). We refer to this hierarchy of
motion objectives as the stack of tasks (SoT). The key components of our current implementation are able to handle a hierarchy of inequality objectives while ensuring the dynamic
consistency using inverse dynamics with real-time
performances.
Task Function Approach
Let C be the configuration space, M the task space, Te M
the tangent space of M at some point e, and U an arbitrary
control space linearly linked to the configuration tangent
space Tq C. The definition of a task comprises three components: 1) the task function e: C " M, 2) the reference behavior given by a vector field of M, eo ) ! Te M, and 3) the
differential map G: U " Te M relating the task to the control input. This map defines the direct relation between the
reference vector field and the control space as eo + d = Gu,
where u is the control signal and d is the task drift. The control law u ) meeting the reference behavior is obtained as
u ) = G # (eo ) + d), where {.} # is any reflexive generalized inverse. Among the possible inverses, the Moore-Penrose pseudoinverse {.} + is generally chosen, but other inverses (such as
weighted inverses) can also be used.
This generic formulation directly corresponds to IK, with
U = Tq C, u = qo , and G = ^2e/2qh as the task function tangent application. The formulation also covers inverse dynamics [12]: the reference behavior is the expected task
acceleration ep ) ! Te M, and the control input is the joint
torques vector x ! U. In this case, the approach consists of
finding the desired torque control input x * that generates the
reference behavior ep ), the joint acceleration qp being a side
variable that might not need to be explicitly computed [24].
Hierarchical Quadratic Program Solver
Finding the control that satisfies instantaneous constraints is
a quadratic problem (QP) whose least-norm solution is given
by the pseudoinverse. The redundancy of the robot with respect to the task is the linear set of controls that do not cause
any change in the task spaces, and is given by the null-space
of the tangent application G. A hierarchy is typically obtained by projecting the secondary objectives onto the null
spaces of the higher priority objectives. Such a solution is
known for handling tasks written as equalities (typically
reaching or pointing tasks).
But the robot control also needs to handle tasks like joint
angle, speed and power limits, autocollisions, obstacle avoidance, singularity avoidance, balance constraints, and visibility
of landmarks in the field of view or behind occlusion. All of
these tasks are inequalities. Thus, inequalities represent important tasks and need to be considered. Some solutions regarding
inequalities are potential functions [11], damping functions
[3], or clamping [23] for joint angle limits. The problem with
18

IEEE ROBOTICS & AUTOMATION MAGAZINE

DECEMBER 2015

these methods is their inability to deal with inequality constraints at arbitrary levels of the hierarchy.
Consider a generic linear system with n linear equalities or
inequalities of the form b i # C i x # b i, where x is the optimization variable, C i is a coefficient matrix, b i and b i are the
lower and upper bounds respectively, and i = 1, g, n represents the priority in the hierarchy. (With this representation
equalities are a special case when b i = b i, and single-sided inequalities are special cases when b i =-3 or b i =+3.) The
lexicographic order (i) ' (ii) ' (iii) ' g, where (i) has the
highest priority, is used to represent the hierarchy. When considering only one objective, the optimum x satisfying the objective is simply obtained by solving the associated QP. For a
hierarchy of multiple objectives, the system is written as a hierarchical QP (HQP) so that if level i has higher priority over
level i + 1, then level i + 1 should be fulfilled as close as possible but without interfering with level i and the other higher
priority levels.
Our team has shown that the optimum of an HQP can be
defined by a cascade of QPs [10]. At each level i of the hierarchy, the QP gives the solution that satisfies the best level i
under the constraint to preserve the optimum found for the
i - 1 first levels. The method is more a formal definition
than an efficient algorithm, provided that when the cascade
is computed using iterative calls to a QP solver, such an implementation is computationally inefficient. In [6], we have
proposed an HQP solver based on a variation of one classical
QP resolution algorithm, the primal active-set solver. First, a
classical primal active-set algorithm finds the active inequality constraints and turns them into equalities; then, a hierarchized complete orthogonal decomposition is computed to
find the optimum of the partial equality problem. Finally, the
two steps are iterated until convergence.
SoT Using Inverse Dynamics
One of the major interests of OSID is to handle limited actuation systems. Humanoid robots, which are floating-based systems, are a particular case of underactuated systems due to
the partial actuation that is completed when they make contact with the environment. The configuration of a humanoid
robot is represented by generalized coordinates q = (x b, q a),
where x b represents the position and orientation of the robot
free-floating body and q a represents the n actuated joints of
the robot. When the robot is in contact with the environment,
the dynamic equation of the system is
Mqp + b + J Tc fc = S T x,

(1)

where M is the generalized inertia matrix; b is the dynamic
drift vector, including Coriolis, centrifugal, and gravity forces;
fc is the vector of the three-dimensional (3-D) contact forces
applied at the contact points; x c, J c = ^2x c 2qh is the
Jacobian of those contact points; x is the actuated torque vector; and S = [0 I] is a matrix selecting the actuated joints. Let
f 9c and x 9c be the vectors containing only the components of
fc and x c normal to the contact surface. The complementarity

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - December 2015