Adrien Escande

Nicolas Mansard

Pierre-Brice Wieber

Hierarchical Quadratic Programming Part 1: Fundamental Bases

Hierarchical least-square optimization is often used in robotics to inverse a direct function when multiple incompatible objectives are involved. Typical examples are inverse kinematics or dynamics. The objectives can be given as equalities to be satisfied (e.g. point-to-point task) or as areas of satisfaction (e.g. the joint range). This two-part paper proposes a complete solution to resolve multiple least-square quadratic problems of both equality and inequality constraints ordered into a strict hierarchy. Our method is able to solve a hierarchy of only equalities ten time faster than the classical method and can consider inequalities at any level while running at the typical control frequency on whole-body size problems. This generic solver is used to resolve the redundancy of humanoid robots while generating complex movements in constrained environment.

In this first part, we establish the mathematical bases underlying the hierarchical problem and propose a dedicated solver. When only equalities are involved, the solver amounts to the classical solution used to handle redundancy in inverse kinematics in a far more efficient way. It is able to handle inequalities at any priority levels into a single resolution scheme, which avoids the high number of iterations encountered with cascades of solvers. A simple example is given to illustrate the interest of our approach. The second part of the paper focuses on the implementation of the solver for inverse kinematics and its use to generate robot motions. (a) (b) Fig. 1: Various situations of inequality and equality constraints. (a) reaching a distant object while keeping balance. The visibility and postural tasks are satisfied only if possible. (b) The target is kept inside a preference area.

Introduction

Least squares are a mean to satisfy at best a set of constraints that may not be feasible. When the constraints are linear, the least squares are written as a quadratic program, whose solution is given for example by the pseudo-inverse. Linear least squares have been widely used in robot control in the frame of instantaneous task resolution [De Schutter and Van Brussel, 1988], inverse kinematics [Whitney, 1972] or operational-space inverse dynamics [Khatib, 1987]. These approaches describe the objectives to be pursued by the robot using a function depending on the robot configuration, named the task function [START_REF] Samson | Robot Control: the Task Function Approach[END_REF]. The time derivative of this function depends linearly on 1 the robot velocity, which gives a set of linear constraints, to be satisfied at best in the least-square sense.

When the constraint does not require the use of all the robot degrees of freedom (DOF), the remaining DOF can be used to perform a secondary objective. This redundancy was first emphasized in [Liégeois, 1977].

Least squares can be used again to execute at best a secondary objective using the redundant DOF [Hanafusa et al., 1981], and by recurrence, any number of constraints can be handled [Siciliano and Slotine, 1991]. Here again, the pseudo-inverse is used to compute the least-square optimum. The same technique has been widely used for redundant manipulator [START_REF] Chiaverini | Kinematically redundant manipulators[END_REF], mobile or underwater [START_REF] Antonelli | Task-priority redundancy resolution for underwater vehicle-manipulator systems[END_REF]] manipulator, multi manipulator [START_REF] Khatib | Vehicle/arm coordination and multiple mobile manipulator decentralized cooperation[END_REF], platoon of mobile robots [Antonelli andChiaverini, 2006, Antonelli et al., 2010],visual servoing [START_REF] Mansard | Task sequencing for sensorbased control[END_REF], medical robots [START_REF] Li | Maintaining visibility constraints during teleechography with ultrasound visual servoing[END_REF], planning under constraints [START_REF] Berenson | Task space regions: A framework for pose-constrained manipulation planning[END_REF], control of the dynamics [START_REF] Park | Contact consistent control framework for humanoid robots[END_REF], etc. Such a hierarchy is now nearly systematically used in humanoid animation [Baerlocher and Boulic, 2004] and robotics [START_REF] Sian | A switching commandbased whole-body operation method for humanoid robots[END_REF], Mansard et al., 2007, Khatib et al., 2008].

Very often, the task objectives are defined as equality constraints.

On the opposite, some objectives would naturally be written as inequality constraints, such as joints limits [Liégeois, 1977, Chaumette andMarchand, 2001], collision avoidance [Marchand andHager, 1998, Stasse et al., 2008a], singularities avoidance [START_REF] Padois | Kinematic and dynamic model-based control of wheeled mobile manipulators: a unified framework for reactive approaches[END_REF], Yoshikawa, 1985] or visibility [START_REF] Garcia-Aracil | Continuous visual servoing despite the changes of visibility in image features[END_REF], Remazeilles et al., 2006] (see Fig. 1).

A first solution to account for these tasks is to embed the corresponding inequality constraint into a potential function [Khatib, 1986] such as a log barrier function, whose gradient is projected into the redundant DOF let free by the first objective [Liégeois, 1977, Gienger et al., 2006].

The potential function simultaneously prevents the robot to enter into a forbidden region and pushes it away from the obstacles when it is coming closer. The potential function in fact transforms the inequality into an equality constraint, that is always applied even far from the obstacle. However, this last property prevents the application of this solution for top-priority constraints.

To ensure that avoidance is realized whatever the situation (and not only when there is enough DOF), several solutions have been proposed, that try to specify inequality objectives as higher-priority tasks.

In [START_REF] Nelson | Strategies for increasing the tracking region of an eye-in-hand system by singularity and joint limits avoidance[END_REF], a trade-off between the avoidance and the motion objectives was performed. In [START_REF] Chang | A weighted least-norm solution based scheme for avoiding joints limits for redundant manipulators[END_REF], the joint limit avoidance was used as a damping factor to stop the motion of a joint close to the limit. Both solutions behave improperly when the motion objective and the avoidance criteria become incompatible. In [Mansard and Chaumette, 2007a], the constraints having priority were relaxed to improve the execution of the avoidance constraint, set at a lower priority level. However, this solution is only valid far from the target point, and the obstacle may finally collide at the convergence of the motion task. In the cases where damping is not sufficient, clamping was proposed [START_REF] Raunhardt | Progressive clamping[END_REF]. It was applied for example to avoid the joint limits of a humanoid [Sentis, 2007]. However, this solution requires several iterations and might be costly. Moreover, it is difficult to relax a DOF that was clamped. In [START_REF] Mansard | A versatile generalized inverted kinematics implementation for collaborative working humanoid robots: The stack of tasks[END_REF], it was proposed to realize an homotopy between the control law with and without avoidance. A proper balance of the homotopy factors ensures that the objectives having priority are respected. However, the cost of this solution in complex cases is prohibitive.

Alternatively, the inequality constraint can be included in the least-square program as it is [Nenchev, 1989, Sung et al., 1996, Decré et al., 2009].

Such a solution is very popular for controling the dynamic of the simulated system [START_REF] Collette | Dynamic balance control of humanoids for multiple grasps and noncoplanar frictional contacts[END_REF], Salini et al., 2009, Bouyarmane and Kheddar, 2011]. In [Kanoun et al., 2011], a first solution to account for inequality constraints in a hierarchy of tasks was proposed. A simplified version was proposed in [De Lasa et al., 2010], that improves the compu-tation cost but prevent the inclusion of inequality except at the top priority. In this paper, we propose a complete solution based on [Kanoun et al., 2011] to account for inequality at any level of a hierarchy of least-square quadratic program, while keeping a low computation cost. The contributions of this paper are:

• we propose the first method to solve a hierarchical quadratic program using a single dedicated solver.

• this solver allows to solve a problem of 40 variables (the typical size on a humanoid robot controlled in velocity) in 0.2ms when only equality constraints are set (up to 10 times faster than the state of the art) and in certified 1ms with inequalities, which could allow to do real-time whole-body control at 1kHz.

• we give the analytical computation cost and prove the continuity and the stability of the control laws derived from this solver.

• we give a complete experimental study of the performances of our method against state-of-theart solvers and demonstrate its capabilities in the robotics context.

The two parts of the paper are structured as follows. The first part establishes the decomposition and the active-search algorithm dedicated to the hierarchical structure. We first recall the mathematical bases of the previous works in Sec. 2. Our solution requires to rewrite the classical hierarchy of equality constraints [Siciliano and Slotine, 1991] in a more efficient manner, which is done in Sec. 3. Then, the algorithm to resolve the hierarchical quadratic leastsquare program in the presence of inequalities is proposed in Sec. 4. Finally, a small example is given in Sec. 5 as an illustration of the use of the solver in the robotics context. The second part focuses on the implementation of the solver and its use to generate motion by inverse kinematics, while providing a more complete set of experimentations to demonstrate the performances of the method.

2 Hierarchies of tasks 2.1 Context: the task-function approach

The task-function approach [START_REF] Samson | Robot Control: the Task Function Approach[END_REF] is an elegant solution to express the objectives to be performed by the robot and deduce from this expression the control to be applied at the joint level. Consider a robot defined by its configuration vector q and whose control input is the joint velocity q. A task function is any derivable function e of q. The image space of e is named the task space. Its jacobian is denoted J = ∂e ∂q . The evolution in the task space with respect to the robot input is given by: ė = J q

(1)

The objective to be accomplished by the robot can then be expressed in the task space by giving a reference task velocity ė * . Computing the robot input boils down to solving the following quadratic leastsquare program (QP):

Find q * ∈ Arg min q J qė *

(2) Such a QP formulation can also be encoutered to inverse the system dynamics in the operational space [Khatib, 1987, Collette et al., 2007], compute a walking pattern [START_REF] Herdt | Online walking motion generation with automatic footstep placement[END_REF] or optimize a linearized trajectory of the robot whole body [START_REF] Pham | Affine trajectory deformation for redundant manipulators[END_REF].

More generically, we consider in this article the case where a robot needs to satisfy a set of linear equality constraints Ax = b. In case this set of constraints is not feasible, it has to be satisfied at best in the least-square sense:

Find x * ∈ Arg min x Ax -b (3)
Among the possible x * , the solution that minimizes the norm of x * is given by the pseudo-inverse:

x * = A + b (4)
where A + is the (Moore-Penrose) pseudo-inverse of A, which is the unique matrix satisfying the following four conditions:

AA + A = A (5)
A + AA + = A + (6) AA + is symmetric (7)

A + A is symmetric (8)
The set of all the solutions to (3) is given by [Liégeois, 1977]:

x * = A + b + P x2 ( 9
)
where P is a projector on the null space of A (i.e. such that AP = 0 and P P = P ), and x2 is any arbitrary vector of the parameter space, that can be used as a secondary input to satisfy a second objective.

2.2 Hierarchy of equality constraints [Siciliano and Slotine, 1991] In this paper, we consider the case where p linear constraints (A 1 , b 1 ) ... (A p , b p ) have to be satisfied at best simultaneously. If the constraints do not conflict, then the solution is directly obtained by stacking them into a single constraint:

A p =    A 1 . . . A p    , b p =    b 1 . . . b p    (10)
The resulting QP can be solved as before. If the constraints are conflicting, a weighting matrix Q is often used to give more importance to some constraints with respect to others or to artificially create a balance between objectives of various physical dimensions: min

x (A p x -b p ) T Q(A p x -b p ) (11)
Rather than selecting a-priori values of Q, it was proposed in [Siciliano and Slotine, 1991] to impose a strict hierarchy between the constraints. The first constraint (with highest priority) (A 1 , b 1 ) will be solved at best in the least-square sense using (4). Then the second constraint (A 2 , b 2 ) is solved in the null space of the first constraint without modifying the obtained minimum of the first constraint. Introducing (9) in A 2 x = b 2 , a QP in x2 is obtained:

min x2 A 2 P 1 x2 -(b 2 -A 2 A + 1 b 1 ) (12)
The generic solution to this QP is:

x * 2 = (A 2 P 1 ) + (b 2 -A 2 A + 1 b 1 ) + P2 x3 (13)
with P2 the projector into the null space of (A 2 P 1 ) + and x3 any vector of the parameter space that can be used to fulfill a third objective. The complete solution solving (A 1 , b 1 ) at best and (A 2 , b 2 ) if possible is:

x * 2 = A + 1 b 1 + (A 2 P 1 ) + (b 2 -A 2 A + 1 b 1 ) + P 2 x3 (14)
where x * 2 denotes the solution for the hierarchy composed of the two first levels, and P 2 = P 1 P2 is the projector over A 2 . This solution can be extended recursively to solve the p levels of the hierarchy [Siciliano and Slotine, 1991]:

x * p = p k=1 (A k P k-1 ) + (b k -A k x * k-1 ) + P p xp+1 ( 15
)
with P 0 = I, x 0 = 0 and P k = P k-1 Pk the projector into the null space of A k [Baerlocher and Boulic, 2004].

xp+1 is any vector of the parameter space that denotes the free space remaining after the resolution of the whole hierarchy.

Projection versus basis multiplication

Given a basis Z 1 of the null space of A 1 (i.e. A 1 Z 1 = 0 and Z T 1 Z 1 = I), the projector in the null space of A 1 can be written P 1 = Z 1 Z T 1 . In that case, it is easy to show that

(A 2 P 1 ) + = (A 2 Z 1 Z T 1 ) + = Z 1 (A 2 Z 1 ) + (16)
The last writing is more efficient to compute than the first one due to the corresponding matrix size. Then, (15) can be rewritten equivalently under a more efficient form:

x * p = p k=1 Z k-1 (A k Z k-1 ) + (b k -A k x * k-1 ) + Z p z p+1
(17) where Z k is a basis of the null space of A k and z p+1 is a vector of the dimension of the null space of A p . This observation was exploited in [START_REF] Escande | Fast resolution of hierarchized inverse kinematics with inequality constraints[END_REF] (which constitute a preliminary version of this work) to fasten the computation of (15).

Inequalities inside a cascade of

QP [Kanoun et al., 2011] The problem ( 3) is an equality-only least-square quadratic program (eQP). Searching a vector x that satisfy a set of linear inequalities is straightforward to write:

Find x * ∈ {x, s.t. Ax ≤ b} (18)
If the polytope defined by Ax ≤ b is empty (the set of constraints is infeasible), then x * can be searched as before as a minimizer in the least-square sense.

The form (3) can be extended to inequalities by introducing an additional variable w, named the slack variable, in the parameter vector:

min x,w w (19) subject to Ax ≤ b + w (20)
The slack variable can relax the constraint in case of infeasibility [START_REF] Hofmann | Exploiting angular momentum to enhance bipedal center-of-mass control[END_REF]. This QP is named a inequality QP (iQP, by opposition to the eQP). In the remaining of the paper, we keep this reduced shape with only upper bound, since it encompasses lower bounds Ax ≥ b, double bounds b -≤ Ax ≤ b + and equalities Ax = b by set-

ting respectively -Ax ≤ -b, -A A x ≤ -b - b + and -A A x ≤ -b b
. Such an iQP can be solved, for example, using an active-search method (recalled in App. A).

The work in [Siciliano and Slotine, 1991] is limited to a hierarchy of eQP. In [Kanoun et al., 2011], a complete solution to extend the hierarchy to inequality constraints was proposed. The method begins with minimizing the violation w 1 of the first level of constraints in a least-squares sense through ( 19). This gives a unique optimal value w * 1 since the cost function is strictly convex in w 1 . It proceeds then in minimizing the violation of the second level of constraints in a least-squares sense:

min x,w2 w 2 (21) subject to A 1 x ≤ b 1 + w * 1 (22) A 2 x ≤ b 2 + w 2 (23)
The first line of constraints ( 22) is expressed with respect to the fixed value w * 1 obtained from the first QP, which ensures that the new x will not affect the first level of constraints and therefore enforces a strict hierarchy. In that sense, ( 22) is a strict constraint while ( 23) is a relaxed constraint. The same process is then carried on through all p levels of priority.

Reduction of the computation

cost [De Lasa et al., 2010] The solution [Kanoun et al., 2011] makes it possible to solve hierarchies of iQP. However, it is very slow, since each constraint k is solved at the iQP of level k and all the following ones. In particular, the first constraint is solved p times.

In [De Lasa et al., 2010], a solution is proposed to lower the computation cost by reducing the generic nature of the problem studied in [Kanoun et al., 2011]: inequalities are considered only at the first level, and this level is supposed feasible. This hypothesis reduces the expressiveness of the method, forbidding the use of weak constraints such as visibility or preference area. However, this expressivity reduction enables to obtain very impressive result for walking, jumping or, as shown in [START_REF] Mordatch | Discovery of complex behaviors through contact-invariant optimization[END_REF], for planning contacts and manipulation.

The first iQP of the cascade does not need an explicit computation, since w * 1 = 0 by hypothesis. Then each level k > 2 is solved in the null space of the levels 2 to k -1: min

z k ,w k w k (24) subject to A 1 (x * k-1 + Z k-1 z k ) ≤ b 1 (25) A k (x * k-1 + Z k-1 z k ) = b k + w k (26)
where x * k-1 is the optimal solution for the k -1 first levels, and Z k-1 is the null space of the levels 2 to k -1. The solution in the canonical basis after the

k th QP is set to x * k = x * k-1 + Z k-1 z * k . The Z k basis is computed from Z k-1 and a singular value decom- position (SVD) of (A k Z k-1 ).
If the first level is empty (or equivalently, if the bounds are wide enough for never being activated), this method is equivalent to (17). The global working scheme of the method [De Lasa et al., 2010] is the same as [Kanoun et al., 2011] since both rely on a cascade of QP computed successively for each level of the hierarchy. The method of [De Lasa et al., 2010] is faster since each QP is smaller than the previous one (the dimension of z k decreases with k), while each QP of [Kanoun et al., 2011] was bigger than the previous ones (the number of constraints increases). The method of [De Lasa et al., 2010] requires an additional SVD, but this could be avoided since the SVD or an equivalent decomposition is already computed when solving the corresponding QP.

However, both methods [Kanoun et al., 2011] and [De Lasa et al., 2010] have the same intrinsic problem due to the nature of the underlying active search algorithm1 . Basically, it searches for the set of active constraints, that holds as equality at the optimum. At each new QP of the cascade, the optimal active set may be completely different. The active search may then activate and deactivate a constraint several times when moving along the cascade, and the succession of all these iterative processes appears to be very inefficient in the end.

Typical examples of this situation are given in Section 5 and in the second part of the paper. Consider a humanoid robot that should keep its center of mass inside the support polygon, put its right hand in the front and its left hand far in the back: when solving the right-hand constraint, the center of mass will saturate in the front, which activates the corresponding constraint. The front constraint is then deactivated when the left-hand constraint brings the center of mass on the back, while the back of the support polygon becomes active. The back constraint may even be deactivated if a last level is added that regulates the robot posture.

Strict hierarchies and lexicographic order

By minimizing successively w 1 , w 2 until w p , the above approaches end up with a sequence of optimal objectives w * 1 , w * 2 , . . . , w * p which is itself minimal with respect to a lexicographic order: it is not possible to decrease an objective w k without increasing an objective w j with higher priority (j < k). Considering a hierarchy between these objectives or a lexicographic order appear to be synonyms. The above approaches can therefore be summarized as a lexicographic multi-objective leastsquares quadratic problem, looking for lex min

x,w1...wp { w 1 , w 2 , . . . , w p } .

(

) subject to ∀k = 1 : p, A k x ≤ b k + w k 27
The earliest and most obvious approach to solve such lexicographic multi-objective optimization problems is to solve the single-objective optimization problems successively [Behringer, 1977], exactly in the same way described above [Kanoun et al., 2011, De Lasa et al., 2010]. This is a very effective approach, but in the case of inequality constraints, each optimization problem requires an iterative process to be solved, and the succession of all these iterative processes appears to be very inefficient in the end.

Following the example of the lexicographic simplex method proposed in [Isermann, 1982], our approach is to adapt the classical iterative processes for optimization problems with inequality constraints directly to the situation of lexicographic multiobjective optimization, resulting in a single iterative process to solve the whole multi-objective problem at once, what appears to be much more efficient. This is what we are going to develop in the following sections for the problem (27).

3 Equality hierarchical quadratic program

Optimality conditions

At first, we consider an equality-only hierarchical quadratic least-square program (eHQP). It is written as a set of p eQP: at level k, the QP to be solved is written: min

x k ,w k w k (28) subject to A k x k = b k + w k (29) A k-1 x k = b k-1 + w * k-1 (30)
where A k-1 , b k-1 and w * k-1 are the matrix and vectors composed of the stacked quantities of levels 1 to k -1 (by convention, they are empty matrix and vectors for k -1 = 0). w * k-1 is the fixed value obtained from the previous QP. The Lagrangian of this problem is:

L k = 1 2 w T k w k + λ T k (A k-1 x k -b k-1 -w * k-1 ) + λ T k (A k x k -b k -w k ) (31)
where λ k and λ k are the Lagrange multipliers corresponding respectively to (30) and ( 29). Differentiating the Lagrangian over the primal variables x k , w k and dual variables λ k , λ k gives the optimality conditions:

w k = A k x k -b k (32) A k-1 x k = b k-1 + w * k-1 (33) λ k = w k (34) A T k-1 λ k = -A T k w k (35)
The two first lines give the condition to compute the primal optimum. From (34), we see that w is indeed at the same time a primal and a dual variable. The last equation gives the condition to compute the dual optimum.

Complete orthogonal decomposition

In the first level, ( 30) and ( 33) are empty. The primal optimum is computed by minimizing w 1 in (32), that is to say by a classical pseudo-inverse of A 1 . The pseudo-inverse can be computed by performing a complete rank revealing decomposition.

Typically a SVD can be chosen. Alternatively, a complete orthogonal decomposition (COD) can be used [Golub and Van Loan, 1996]:

A 1 = V 1 U 1 0 0 L 1 0 Y 1 Z 1 T = U 1 L 1 Y T 1 (36) where W 1 = V 1 U 1 and Y 1 Z 1 are two or- thonormal matrices, U 1 being a basis of the range space of A 1 , Z 1 of its kernel and L 1 is a lower tri- angular matrix whose diagonal is strictly nonzero. If the first level (A 1 , b 1 ) is feasible, then A 1 is full row rank and U 1 is the identity (V 1 is empty). In that case, (36) is the QR decomposition of A T 1 (or LQ decomposition of A 1 ).
The COD is cheaper to compute than the SVD. The algorithms to compute it involve a rather simple serie of basic transformations (Givens or Householder rotations) and are known to be nearly as robust as the algorithms computing the SVD (and much easier to implement). It is one of the classical ways to solve rank deficient quadratic least-squares problems [Björck, 1996].

The pseudo-inverse of A 1 now only implies the easily computable inversion of L 1 :

A + 1 = Y 1 Z 1 0 0 L -1 1 0 V 1 U 1 T = Y 1 L -1 1 U T 1 (37)
The optimal solution x * 1 is obtained by

x * 1 = A + 1 b 1 = Y 1 L -1 1 U T 1 b 1 (38)
Rather than computing the explicit pseudo-inverse, the optimum should be computed by realizing a forward substitution of L 1 on U T 1 b 1 . The corresponding slack variable is:

w * 1 = A 1 x * 1 -b 1 = U 1 U T 1 b 1 -b 1 = -V 1 V T 1 b 1 (39)

Hierarchical complete orthogonal decomposition

Consider now the second level of the hierarchy (28) (k = 2). As in Sec. 2.3, condition (33) can be rewritten using ( 38) and (39) as:

x 2 = x * 1 + Z 1 z 2 (40)
where z 2 is any parameter of the null space of A 1 . Condition ( 32) is then written:

w 2 = (A 2 Z 1 )z 2 -(b 2 -A 2 x * 1 ) (41) = A 2 Y 1 A 2 Z 1 Y T 1 x * 1 z 2 -b 2 (42) because Y 1 Y T 1 x * 1 = x * 1 .
The matrix A 2 is in fact separated in two parts along the Y 1 , Z 1 basis: the first part A 2 Y 1 corresponds to the coupling between the two first levels. The corresponding part of the parameter space has already been used for the level 1 and can not be used here. The second part A 2 Z 1 corresponds to the free space that can be used to solve the second level.

The optima x * 2 and w * 2 are obtained by performing the pseudo-inverse of A 2 Z 1 using its COD:

(A 2 Z 1 ) = V 2 U 2 0 0 L 2 0 Ỹ2 Z2 T ( 43
)
The basis W 2 = V 2 U 2 provides a decomposition of the image space of A 2 along its range space and the orthogonal to it. The basis Y 2 Z 2 = Z 1 Ỹ2 Z2 is in fact another basis of the null space of A 1 that also provides a separation of the kernel of A 2 . In particular, Z 2 is a basis of the null space of both A 1 and A 2 that can be used to perform the third level. The optimum x * 2 is finally:

x * 2 = x * 1 + Z 1 (A 2 Z 1 ) + (b 2 -A 2 x * 1 ) + Z 2 z 3 (44) = x * 1 + Y 2 L -1 2 U T 2 (b 2 -A 2 x * 1 ) + Z 2 z 3 (45) = Y 1 Y 2 Z 2   Y T 1 x * 1 Y T 2 x * 2 z 3   (46) where x * 2 = Y 2 L -1 2 U T 2 (b 2 -A 2 x * 1
) is the contribution of the second level to the optimum and z 3 is any parameter of the null space Z 2 used to perform the following levels. The optimum w * 2 is directly obtained using (42). This can be written using the two basis

V 2 , U 2 and Y 1 , Y 2 , Z 2 : w * 2 = V 2 U 2 N 2 0 0 M 2 L 2 0   Y T 1 x * 1 Y T 2 x * 2 z 3   -b 2 (47) with M 2 = U T 2 A 2 Y 1 and N 2 = V T 2 A 2 Y 1 the cou- pled part of A 2 corresponding
respectively to its feasible space U 2 and its orthogonal, and using

Y T 2 x * 2 = Y T 2 x * 2 .
When the projected matrix A 2 Z 1 is full row rank, V 2 and N 2 are empty. If the ranks of A 2 and A 2 Z 1 are the same, then N 2 is zero. This case is sometimes called a kinematic singularity [Chiaverini, 1997]. Alternatively, the rank of A 2 can be greater than A 2 Z 1 . In that case, N 2 is nonzero, and the singularity is caused by a conflict with the level having priority. This case is sometimes called an algorithmic singularity [Chiaverini, 1997].

In (47) a decomposition of the matrix A 2 appears, that can be written generically for any k ≥ 2:

A k = V k U k N k 0 0 M k L k 0 Y k-1 Y k Z k T (48) = W k H k Y T k (49) with N k = V T k A k Y k-1 , M k = U T k A k Y k-1 , Y k-1 = Y 1 . . . Y k-1 and H k = N k 0 M k L k . Similarly to the second level, N k is empty if A k Z k-1 is full row rank;
it is zero if the rank deficiency is inherent to the level (kinematic singularity) and nonzero if the rank deficiency is due to a conflict with a level having priority (algorithmic singularity). In that third cases, the stronger the norm of a column of N k , the stronger the coupling of the unfeasible part with the corresponding higher level. The strongest columns of N k can then indicate the conflicting levels that prevent the realization of level k. Stacking all the decompositions (48) for the k first levels, a single decomposition of A k is recursively ob-tained by:

A k-1 A k = W k-1 0 0 W k   H k-1 0 0 N k 0 0 M k L k 0   [Y k-1 Y k Z k ] T = W k H k Y T k (50)
The complete form for the p levels is finally:

   A 1 . . . A p   =    W 1 . . . W p                     0 0 0 0 0 L 1 0 0 0 0 N 2 M 2 0 0 0 0 L 2 0 0 0 . . . . . . . . . . . . N p M p 0 0 L p 0                  Y T with Y = Y p Z p .
If all the levels are feasible, all the matrices A k and A k Z k-1 are full row rank and all the N k matrices are empty. In this case, the decomposition is a COD of A p . If there is no conflict between the levels, all the N k are zero. In this case, it is only a matter of row permutations to turn the above decomposition into a perfect COD of the matrix A p , involving an invertible lower triangular matrix. This decomposition, which has been designed to enforce a strict hierarchy between different priority levels, looks close to a classical COD and is indeed a COD in particular cases. For this reason, we propose to call this decomposition a Hierarchical Complete Orthogonal Decomposition (HCOD) of the matrix A p . We have seen in ( 45) that the primal optimum of the second level x * 2 is directly computed from H 2 and x * 1 . Using the same reasoning, the optimum of level k, given by (17), is computed using H k :

Primal optimum and hierarchical inverse

x * k = x * k-1 + Y k L -1 k U T k (b k -A k x * k-1 ) + Z k z k+1 (51)
The least norm solution for x * k is obtained for every z i+1 = 0, what we suppose from now on. As observed in this equation, the optimum of each level k is computed using the level k of the HCOD and the optimum of level k -1. By recurrence, x * p can be computed directly using the HCOD, by reformulating (51) in the following way:

x * p = A ‡ p b p (52)
where A ‡ p is defined by a matrix recursion:

A ‡ k = (I -Y k L -1 k U T k A k )A ‡ k-1 Y k L -1 k U T k ( 53
)
Or more simply, with the HCOD:

A ‡ p = Y p H ‡ p W T p ( 54
)
with

H ‡ k = H ‡ k-1 0 0 -L -1 k M k H ‡ k-1 0 L -1 k (55)
From this last form, we can see that the optimum is structured by layer, following the hierarchy of problems. This structure is more evident when the optimum is computed in the Y basis. The primal optimum in the Y basis is denoted

y * k = Y T k x * k . The contribution x * k -x * k-1 of the level k to the primal optimum is denoted y * k = Y T k (x * k -x * k-1 ) (= Y T k x * k since Y T k x * k-1 = 0)
. Then (51) can be rewritten as:

y * k = y * k-1 y * k ( 56
)
where The obtained matrix A ‡ p respects three of the four conditions of Moore-Penrose (5)-(8). First, H ‡ p H p is easily shown to be equal to the identity matrix (by recurrence, starting from

y * k = L -1 k (U T k b k -M k y * k-1
H ‡ 1 H 1 = L -1 1 L 1 = I). Then: A ‡ p A p = Y p Y T p = p k=1 Y k Y T k (57)
This matrix product is obviously symmetric: A ‡ p respects (8). From this result, the condition (5) is easily demonstrated:

A p A ‡ p A p = W p H p Y T p Y p Y T p = A p (58)
With the same argument, ( 6) is also respected. The last condition ( 7) is not respected in general:

H k H ‡ k =   H k-1 H ‡ k-1 0 0 N k H ‡ k-1 0 0 0 0 I   (59) Since H ‡ k-1 is full row rank, the term N k H ‡ k-1 is zero iff N k is zero,
that is to say if level k does not conflict with the above hierarchy. If all the N 1 ...N k are zero, the fourth property ( 7) is then also respected:

A ‡
p is strictly the pseudo-inverse of A p and the optimum (51) enforcing a strict hierarchy between the different priority levels appears to be equal to a classical least-squares solution to A k x = b k regardless of any hierarchy.

In general, the matrix A ‡ p respects only three of the four properties of Moore-Penrose. This matrix is a reflexive generalized inverse of A p . It has been designed to enforce a strict hierarchy between different priority levels and looks close to a pseudo-inverse. For this reason, we propose to call this matrix the hierarchical inverse of the matrix A k .

Damped inverse

The hierarchical inverse computes the same solution than the classical method [Siciliano and Slotine, 1991] and is thus subject to the same singularities [Chiaverini, 1997]. These singularities are studied in the second part (see [Escande et al., 2012], Sec. 4.3). A singularity is reached when one of the diagonal element of a L k becomes zero. The corresponding row is then moved from M k to N k . In the neighborhood of the singularity, the diagonal element of L k is small and can produce a large response in x * k . This is generally a desirable feature in numerical optimization (since it corresponds to an accurate solution of a ill-conditionned problem) but is often undesirable in robotics. In that case, a damped inverse of L k can be used [Wampler, 1986, Deo and Walker, 1992, Sugihara, 2011]:

L †η k = (L k L T k + η 2 I) -1 L T k = L k ηI + I 0 (
w * k = V k N k y * k-1 -V k V T k b k (61) = V k V T k (A k x * k-1 -b k ) (62)
The slack of level k does not depend on the optimum x * k of the same level. It is equal to the projection of b k into the orthogonal to the range space, from which the contribution of the previous levels is subtracted. In particular, if there is no conflict between the levels, the slack is directly obtained by projecting b k . Of course, if level k is not in singularity (intrinsically or due to the previous levels), V k is empty and the slack is zero. A singular point is obtained when

V k is not empty but A k x * k-1 = V k V T k b k : in that
case, the level is conflicting with the previous ones, but it is exactly realized by the contribution of the above hierarchy.

Finally, the algorithm to compute x * p and w * p is summarized in Alg. 1.

Transposed hierarchical inverse and dual optimum

At level k, the dual optimum is given by (35), recalled here:

A T k-1 λ k = -A T k w k A solution (
the least-square one) to this second optimality condition can be obtained with the hierarchical inverse:

λ k = -A ‡T k-1 A T k w * k (63)
e = U T k b k -M k y * k-1 7: w * k = V k (N k y * k-1 -V T k b k ) 8: e := L -
A T k-1 λ k = -A T k-1 A ‡T k-1 A T k w * k = -Y k-1 Y T k-1 A T k w * k (64)
This last form is equal to A T k w * k since, from (48) and (61), we have:

A T k w * k = Y k-1 N T k (N k y * k-1 -V T k b k ) (65)
For each level k, there is a multiplier λ k that corresponds to all the level of higher priority. There is no real sense in stacking the multipliers of each levels. They can be summarized under a pseudo matrix structure:

Λ p =          w * 1 1 λ 2 1 λ 3 . . . 1 λ p-1 1 λ p w * 2 2 λ 3 . . . 2 λ p-1 2 λ p w * 3 . . . 3 λ p-1 3 λ p . . . . . . w * p-1 p-1 λ p w * p          (66)
where j λ k (j < k) denotes the components of the multipliers λ k of the level k for the constraints of level j and the empty spaces for j > k express the absence of multipliers on above levels.

There is no direct formulation to compute the whole Λ p . Alternatively, the multipliers of each level have to be computed iteratively. The solution (63) gives the matrix formulation of the Lagrange multipliers of level k. To compute the multipliers, it is more efficient to avoid the explicit computation of the hierarchical transpose inverse. Using ( 53), (63) can be rewritten:

λ k =    1 λ k . . . k-1 λ k    = -A ‡T k-2 (A T k w * k + A T k-1 k-1 λ k ) -U k-1 L -T k-1 Y T k-1 A T k w * k
(67) By recurrence, the components j λ k of the multipliers of level k can be computed starting from j = k -1 down to 1.

j λ k = -U j L -T j Y T j k-1 i=j+1 A T i i λ k +A T k w * k (68)
Using the HCOD structuration of the A T i i λ k , the sum on the right part can be computed for a reduced cost. The algorithm is given in Alg. 2. The cumulative variable ρ is used to propagate the recursion across the k levels. At the end of any iteration j, the following property is respected:

ρ (j) = Y T j k-1 i=j+1 A T i i λ k +A T k w * k (69)
In line #7, ρ (j+1) is separated in two parts following the separation of

A j+1 = A j A j+1
. The first part of the vector is used to satisfy (69) while the second part gives j λ k using (68).

While the primal algorithm 1 computes the primal optima x and w for all the levels at the same time, the dual algorithm 2 can only achieve the computation of w and λ for one level at a time. Both algorithms can compute w naturally. If both are used, then a choice has to be made on where to really perform the computation of w * .

Conclusion

In this section, we proposed a complete solution to compute the primal and dual solutions of a hierarchical quadratic least-square program with only ρ := ρ -M T j r 12: end for 13: return w * k , λ k equality constraints. The solved problem is in fact nothing more than the problem already solved in [Siciliano and Slotine, 1991]. The proposed solution enables much faster computation, thanks to the use of a COD instead of a classical SVD but also mainly because of the form Z k-1 (A k Z k-1 ) + that is much more efficient than the classical (A k P k-1 ) + . The underlying structures that we have emphasized are also interesting by themselves, for the information they reveal on the problem structure: the HCOD can be used to study the conflicts between the level of the hierarchy, and the optimum y * reveals the hierarchy structure of the optimal solution. Finally, we have proposed a solution to compute the Lagrange multipliers of the hierarchical problem. These multipliers also provide a lot of information about the problem organization: for example, if one level is not properly satisfied because of a conflict, the multipliers can help to select which level to remove to enhance its resolution. Lagrange multipliers are also mandatory for the active-search algorithm proposed in the next section.

Inequality hierarchical quadratic program

We now consider the hierarchical problem ( 27) subject to inequality constraints.

Available options and principle

The two main classes of algorithms to find the optimum of a quadratic cost function over inequality constraints are the interior-point methods (IPM) and the active-search algorithms. IPM convert the quadratic problem with inequality constraints into a non-linear parametric problem without inequality, and iterates a deterministic number of times toward the optimum [Nemirovski, 1994].

On the opposite, active-search algorithms iterate to find the set of constraints that hold as equalities at the optimum, named the active set. Convergence in finite time is certified by making sure that the objective function is strictly decreasing at each iteration. This approach led to the simplex method in the case of linear programming [START_REF] Dantzig | Linear programming: theory and extensions[END_REF] or active-set methods in the case of quadratic programming. The number of iterations is non-deterministic, but can be small if a good initial guess of the optimal active set is known. Such a knowledge is more difficult to use with IPM. For example, in sequential quadratic programming, active searches are often preferred to IPM due to this property. In robotics the optimal active set is slowly evolving over time, what makes active-search algorithm more suitable than IPM.

Active-search methods are themselves divided mainly in two classes. The primal active search starts with a feasible solution (which is sometimes costly to compute), and maintains the feasibility over all the iterations. The dual active search does not need a feasible start, but the feasibility is only met at the optimum. Primal algorithm is interesting in the perspective of real-time implementation, since it can be interrupted at any time and returns a consistent solution. This property will be explored in the second part of the paper (see [Escande et al., 2012], Sec. 4.4).

In this section, we propose to extend the classical primal active-search algorithm (recalled in App. A) to compute the optimal active set and the corresponding optimum of a HQP with inequalities (or iHQP by opposition to the eHQP). To avoid the unnecessary iterations encountered in [Kanoun et al., 2011, De Lasa et al., 2010], the active sets of all the hierarchical levels are computed at the same time. We first propose a rewriting of the classical active search for an iQP. From this reformulation, the original iHQP algorithm will be established.

Dedicated active search for the two first levels

We consider the reduced case with only two levels and the first level strictly feasible (case ( 27) with p = 2 and w * 1 = 0): min

x,w2 w 2 (70) subject to A 2 x ≤ b 2 + w 2 (71) A 1 x ≤ b 1 (72)
This iQP can be solved using a classical active-search. However, this would not take into account the specific form of the variable w 2 . Indeed, the slack variable w 2 is one of the variable to be optimized. Theoretically, it has the same meaning as the variable x. In practice, its specific role in the constraint equations can be taken into account to reduce the amount of computation. In particular, an algorithm computing the solution of ( 70) can be used directly to compute the optimum of a problem without slacks with no efficiency lost. We propose below a modified version of the classical active-search algorithm (recalled in App. A). This algorithm is an intermediary result that will be used to build and justify the hierarchical algorithm proposed in the following section.

Algorithm principle

For a given x,

a constraint A r , b r is satisfied if A r x ≤ b r , violated if A r x > b r and saturated if A r x = b r .
The algorithm maintains a set of constraints that are saturated (for level 1) or should be minimized (for level 2) and thus hold as equalities.

This set is called the active set and is denoted by S. For a given active set S, the associated eHQP is obtained by selecting the rows of S. The two first algorithms are trivially adapted and denoted by eHQP primal(A p , b p , S) and eHQP dual(A p , b p , S).

The algorithm starts from an initial feasible point.

At each iteration, the corresponding eHQP is solved, and depending on the result, the active set is modified. See App. A for details. First, the initial point needs only to satisfy (72), since for any x, it is easy to build w 2 so that ( 71) is satisfied. Among the possible w 2 , the initial one is chosen with zeros on the components corresponding to inactive constraints. This property is kept along the iterations: the inactive components of w 2 are null. It is preserved at each step (73). It is of course preserved when activating any constraint and deactivating a constraint of A 1 . When deactivating a constraint of A 2 , the corresponding w 2 (which is also the Lagrange multiplier due to (34)) is strictly negative. Then setting it to zero keeps the constraint satisfaction while strictly decreasing the cost function. With this simple modification of the classical algorithm, the inactive slacks are kept to 0 during all the algorithm.

By recurrence over the algorithm iterations, it is straightforward to show that all the constraints of the second level A 2 x (i) ≤ b 2 are satisfied or active. For the active component, the slack variable satisfies w

(i) 2 = A 2 x (i) -b 2 .
From these observations, we see that it is not necessary to keep track of w 2 since it is nothing more than the residue of the constraints of the second level corresponding to the current x (i) .

Based on these observations, a dedicated version of the classical active-search algorithm that computes the optimum of ( 70) is proposed. It is summarized in Alg. 3 and detailed below.

Initialization

The algorithm starts with an initial point x (0) satisfying the strict level (72) and an initial guess of the active set of the second level S (0) 2 . The active set for the first level is deduced from x (0) . 0) . The iterations of the algorithm are denoted by x (1) ...x (7) . The optimum is finally reached in x (7) . The constraints of the first levels are always respected, and the path is slidding along these constraints. From x (3) , all the constraints of level 2 are satisfied or active. The first deactivation then occurs. The details of the execution are given in App. B.

Step length

The active-search then maintains a value of the parameter x (i) and the active set S (i) . At each iteration, the algorithm first computes the optimum x ( * i) of eHQP associated to S (i) . The current parameter is then moved toward x ( * i) :

x (i+1) = x (i) + τ (x ( * i) -x (i) ) (73)
The length τ of this step is computed for each constraint component by component. The minimum is then selected.

For the strict level A 1 , τ is chosen to prevent inactive constraints of the first level from becoming violated:

τ 1,r = b 1,r -A 1,r x (i) A 1,r (x ( * i) -x (i) ) ( 74
)
with A 1,r and b 1,r the r th row of A 1 and b 1 . If the minimal τ corresponds to τ 1,r , the corresponding constraint is activated.

Similarly for the relaxed level, if

x (i) respects A 2,r x (i) ≤ b 2,r , the step length is computed to satu- rate the constraint. If x (i) does not respect A 2,r x (i) ≤
b 2,r , the constraint was not satisfied at the previous step. A full step τ = 1 can be performed. As previously, the constraint r is activated if τ 2,r is the minimum. If several constraints correspond to the minimum, only one of them should be arbitrarily activated.

In summary, τ 2,r is chosen as:

τ 2,r = b2,r-A2,rx (i) A2,r(x ( * i) -x (i) ) if A 2,r x (i) ≤ b 2,r 1 otherwise ( 75
)
The step-length algorithm is summarized in Alg. 4.

Positivity of the multiplier

If no constraint needs to be activated, it means that the optimum is reached if S (i) is the optimal active set. This is checked by looking at the components of the Lagrange multipliers. The multiplier of the strict level is λ given by ( 63). The multiplier of the relaxed level is w 2 . If the active set is optimal, then all the components of λ and w 2 are non negative. If one of them is negative, the constraint corresponding to the lowest component of the multipliers is deactivated.

Algorithm termination and proof of convergence

The algorithm stops at the first iteration where no constraint needs to be activated or deactivated. The current value of the parameter x (i) is the (primal) optimum of the iHQP. The current value of S (i) gives the optimal active set.

When starting the iterative process, some of the constraints of A 2 may be inactive but also not satisfied. After a finite number of iterations (bounded by the total number of constraints of the problem), all the constraints of A 2 are satisfied or active. This is the end of a serie of iterations where only activations have occured. During this serie, the Lagrange multipliers are never computed: there cannot be any deactivations while some constraints of A 2 are still violated. The behavior of the algorithm is then the same as the classical active-search algorithm and thus is ensured to converge.

Algorithm 3 Active search for a strict and a relaxed constraints

1: Input: -x (0) such that A 1 x (0) ≤ b 1 2:
-S (0) 2 an initial guess for the active set of A 2 3: Output: x * minimizing (70)

4: x = x (0) 5: S = {r s.t. A 1,r x 0 = b 1,r } S (0) 2 6: repeat 7:
--Compute the next optimum--8:

x * = eHQP primal (A 2 , b 2 , S)

9:

--Compute the step length using Alg. 4--

10: τ ,activate,cst = step length(A 2 , b 2 , x, x * )
11:

x

:= x + τ (x * -x) 12:
--If necessary, increase the active set--13:

if activate then end if 23: until not activate and ν > 0 24: return x * := x

Hierarchical active search

The main loop of Alg. 3 is composed of two sets of instructions: the first one (lines #7 to #15) concerns the activation of needed constraints, while the second one (lines #16 to #21) deals with the deactivation to obtain the optimal active set. The first part of the loop is incrementally building an active set such that all the constraints are activated or satisfied. It can be observed that the hierarchical relation between the levels has strictly no importance in this first part. Indeed, any value can be applied in the null space of the last level without disturbing the algorithm. Consequently, it is possible to search for the active set of all the levels at the same time.

Algorithm 4 Computation of the step length of Alg. 3 1: function step length (A, b, x 0 , x 1 ) 2: Input: Constraint A, b, current point x 0 , target optimum x 1 3: Output: step length τ , activation boolean activate, constraint reference cst 4: activate := False 5: for each row r of A do activate := True; cst := arg min{τ } 18: end if 19: return τ , activate, cst However, the same remark is not possible in the second part of the loop. Indeed, the previous section has shown that the primal optimum can be computed for all the levels at the same time. There is however no direct solution to compute all the Lagrange multipliers at once, and each level must be explored successively to compute the corresponding multiplier.

6: if A r x 0 ≤ b r then 7: τ [r] = br-Arx0 Ar(x1-x0) if A r (x 1 -x 0 ) = 0 else 1 8: else 9: τ [r] = 1 10: if A r x 1 > b
These two remarks are used to build the hierarchical active search detailed below and summarized in Alg. 5.

Algorithm principle

The proposed algorithm is composed of two loops: an inner loop that first enforces then maintains the property that all the constraints should be activated or satisfied. And an outer loop that explores all the levels in ascending order to search for the corresponding optimal active set by removing unnecessary constraints.

Initialization

The algorithm starts with an initial guess of the active set of all the levels. It does not need an initial parameter x (0) since none of the levels (even the first one) is guaranteed to be feasible. It then starts with the arbitrary value x (0) = 0.

Step length and activation

At each new inner iteration, the algorithm first computes the optimum of the eHQP associated to the current active set. It then computes the step length following the same rules as in the previous algorithm using ( 75) at all the levels.

Depending on the initial guess S (0) and the corresponding eHQP optimum, some of the inactive constraints may be violated. In that case, the activation loop will first make some full steps τ = 1, while each time adding one violated constraint into the active set. The number of such steps is bounded by the number of rows of A p . At the end of these steps, all the constraints should be activated or satisfied. This property is then maintained throughout all the following iterations.

Positivity of the multipliers and deactivation

When all the necessary constraints have been activated, the multipliers of the first level are computed (it was not necessary to test them before). As before, the active set is optimal with respect to the first level if none of the multiplier components is strictly negative. Otherwise, the constraint corresponding to the lowest component is deactivated, and a new inner iteration is started.

In [Kanoun et al., 2011], it was observed that, if one of the constraint of the first level is not satisfied (the slack variable is then strictly positive), this constraint can be changed into an equality for all the following levels, to avoid undesirable future deactivations. Similarly, when the first optimal active set is found, the multipliers of the second level can be computed and checked, but without deactivating a constraint whose slack at the first level was positive. The active set is optimal with respect to the second level if all the components of the multipliers are non negative or corresponds to a positive slack of the first level.

A constraint of the second level whose slack is positive will also be kept active for all the following levels. For the same reasons as observed in [Kanoun et al., 2011], if one of the constraints of the first level corresponds to a strictly positive component of the multiplier of the second level, this constraint can be locked to prevent any future deactivation.

In summary, the outer loop explores each level starting from the first one. At each level, it computes the multipliers. If a constraint is strictly negative and does not correspond to a strictly positive component of the multipliers of the previous levels, it is deactivated. When no more constraint needs to be deactivated, the constraint corresponding to strictly positive components of the multipliers are stored in the set F of locked constraints.

Algorithm termination and proof of convergence

The outer loop terminates after a fixed number of iterations when all the p levels have been explored. At this point, the active set is such that all the constraints are active or satisfied, and it is optimal for each of the levels.

As in the previous algorithm, the inner loop starts by a series of full step τ = 1 until all the constraints are active or satisfied. Then, for a given iteration k of the outer loop, suppose that the active set of the levels k + 1 to p is constant: the inner loop behaves similarly to the previous algorithm and thus converges. In practice, the active set of the upper levels is not constant, but it can only be increased, and then can vary a finite number of times (bounded by the number of rows of the levels k + 1 to p), which ensures the termination of the inner loop.

Lexicographic optimization

The algorithm deactivates a constraint if there exists a level k for which the component of the multiplier corresponding to the constraint is negative, while it --Compute the next optimum--8:

x * = eHQP primal (A p , b p , S) 9:

--Compute the step length using Alg. 4--10:

τ ,activate,cst = step length(A p , b p , x, x * ) 11:

x := x + τ (x * -x) 12:
--If necessary, increase the active set-- until not activate and ν > 0 29: end for 30: return x * := x is zero for all the multipliers of level j < k. Consider the pseudo-matrix Λ p in (66), recalled here:

Λ p =        w * 1 1 λ 2 . . . 1 λ p-1 1 λ p w * 2 . . . 2 λ p-1 2 λ p . . . . . . w * p-1 p-1 λ p w * p        (66)
A constraint is deactivated iff the corresponding row of Λ p has one strictly negative component on column k preceded by only zeros for the columns j < k. In other words, the row is smaller than zero in the lexicographic sense:

Λ p = 0 ... 0 -α × × ... × ≺ 0 (76)
Using this notation, Alg. 5 can be rewritten on the exact same form than Alg. 3, using a lexicographic test on Λ p instead on lines #18-#19.

If a lexicographical writing of Alg. 3 is simpler, it is more efficient from a computational point of view to execute Alg. 5 since it avoids computing the whole Λ p at each iteration.

Least-square solution

The eHQP algorithm gives the least-square solution among all the solutions of same cost, when z p+1 is set to 0. However, this is not the case of the iHQP since some constraints might be uselessly active. Indeed, there is no mechanism to deactivate the unnecessary constraints if they are not disturbing the optimum. To force the deactivation of the unnecessary constraints, an artificial last level can be added by setting A p+1 = I and b p+1 = 0 that is to say:

x = 0 (77)
This constraint will always be rank deficient, but its satisfaction at best in the least-square sense ensures that the constraints that artificially increase the norm of x are deactivated: the optimal active set is unique and the the returned optimum is the least-square one.

Conclusion

Based on the hierarchical decomposition, we have proposed an active-search algorithm that solves the iHQP problem. On the contrary to the cascade of QP used in [Kanoun et al., 2011, De Lasa et al., 2010], this algorithm computes the active set of all the level of the hierarchy at the same time. It thus solves the complete hierarchical problem at once, avoiding the back-and-forth effects encoutered with cascades and sparing the cost of computation. One of the costly computation of the algorithm is the HCOD. This should be computed once at the start of the loop. Then, the initial decomposition can be updated to follow the changes of the active set. This will be detailed in the second part [Escande et al., 2012].

Example of use

We now briefly illustrate with a small example the use of our solver to generate a motion with a humanoid robot. At each control cycle of the robot, the motion objectives are written as a set of linear equalities and inequalities as formulated in (1). The robot velocity is then computed by inverse kinematics. A more detailed description will be given in the second part of the paper (see [Escande et al., 2012], Sec. 5.2). In the presented motion, the robot HRP-2 is walking beside a companion robot and should keep it below an umbrella carried in its right hand. The objectives to satisfy are • e jl : avoid the joint limits • e pg : track the center-of-mass and flying foot references issued from a walking pattern generator [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF][START_REF] Stasse | Modular architecture for humanoid walking pattern prototyping and experiments[END_REF]] • e θ : keep the angle of the umbrella stick below a fixed threshold • e umb : keep the companion below the umbrella • e f ov : look at the companion head • e θmin : minimize the stick angle

The task e jl , e θ , e umb and e f ov are expressed as inequalities. The constraints corresponding to e θ and e θmin have the same left side, but the first one enables a motion within the given boundaries while the second one tries to prevent any motion by regulating the stick at the vertical. The order of the tasks in the hierarchy follows the list order. The obtained motion is summarized by Figures 3 to 5.

At the beginning, HRP-2 is close to its companion (see Fig. 3-(a)): all the constraints can be satisfied and the companion is safely below the umbrella. HRP-2 has then to step on the right (following a set of given footprints) to avoid an obstacle, which prevents him from staying close to the companion (see Fig. 3-(b)). The task e umb cannot be executed while Fig. 4: Walking with an umbrella: task errors. The companion starts outside of the umbrella shadow and the robot first brings the umbrella to satisfy this objective. When going around the obstacle, the task regulating the angle of the umbrella stick is violated first (T = 5.8s), followed quickly by the FOV task (T = 5.9s). One second later, the task keeping the companion below the umbrella is also violated (T = 6.3s). The angle of the stick then reaches the minimum limit (T = 8.1s). This fourth task having priority over the three others, the angle is blocked. Finally, the robot reaches a region where the tasks are feasible (from T = 15s). satisfying the other tasks. Fig. 4 shows the evolution of the errors of the four tasks of least priority: the task e θmin is first relaxed at T = 5.8s, quickly followed by e f ov and e umb . When the angle of the stick reaches the maximal value authorized by e θ , it stops and remains constant. The robot then continues to walk, with the umbrella slightly tilted and as close as possible to the companion. The tasks become satisfied again as soon as the robot reaches a feasible region. The walking task and the joint limits are satisfied during all the motion, as shown in Fig. 5.

This example illustrates both the hierarchy and the interest of inequalities objectives: when the set of objectives is infeasible, some are perfectly satisfied, while the others, less important, are successively relaxed to keep a safe behavior. This enables to implement in particular avoidance behavior (the joint limits were never violated) and preference area (such as the umbrella shadows) or postural tasks (such as the tilt angle of the stick).

Conclusion

In this first part of the paper, we have proposed a method to solve a hierarchical least-square quadratic problem. First, we have focused on the subproblem where only equalities are involved. In that case, the optimal solution corresponds to what is classically computed in inverse kinematics when handling redundancy by successively projecting the next problem in the null space of the previous ones [Siciliano and Slotine, 1991]. We described a dedicated matrix decomposition, the HCOD, that reveals the structure of the problem and can be used to explain the obtained optimum, for example for automatic supervision. With this decomposition, the optimum is obtained using a single inversion. It can also be used to compute the optimum of the dual problem.

Subsequently, we have used this first solver to build an active-search algorithm that solves the whole problem involving inequalities. Contrary to previous methods [Kanoun et al., 2011, De Lasa et al., 2010] that use a cascade of QP, our algorithm solves the problem for all the levels of the hierarchy at the same time. This avoids activations and deactivations encountered when using a cascade, which is undesirable for computation-time reasons but also to avoid numerical instability and bad numerical behavior of the algorithm in general.

In the second part of the paper [Escande et al., 2012], we focus more on practical implementation of the solver. In particular, a method to compute the HCOD and to update it along the active-search iterations is detailed. The theoretical properties of the solver are given (complexity, continuity, stability). And a strong experimental setup demonstrates the interest of the approach.

A Some basics on the active search

The classical active-search algorithm to solve an iQP is quickly recalled. We consider the following generic least-square form:

min χ Aχ -b (78) subject to Cχ ≤ d (79)
As in the rest of the paper, only upper bounds are considered here.

A.1 Algorithm principle

For a given χ, a row i of (79

) is satisfied if C i χ ≤ d i , violated if C i χ > d i and saturated if C i χ = d i .
The constraint (79) defines a polytope in the parameter space. When the optimum of the unconstraint cost function is outside of this polytope, the optimum of the constrained QP is on the border of the polytope, i.e. at the saturation of a subset of (79), named the optimal active set. The active search iterates on a candidate active set, denoted by S, until it finds the optimal one. The constraints of S are said to be active. For a given S, an associated eQP can be defined by considering only the active constraints as equalities. If we know the optimal active set, the optimum of the iQP is obtained by solving the associated eQP.

This algorithm could be used directly to compute the optimum of a QP with a slack variable, such as the one defined in (70), using χ = (x, w 2 ), but in a less optimal manner than with our method. The algorithm is summarized in Alg. 6 and detailed below.

A.2 Initialization

The active search starts with a feasible point χ (0) that satisfies (79) and an initial guess of S (0) (for example, the set of all the saturated constraints at χ (0) ). The initial point χ (0) is not trivial and requires an iterative process, called Phase I, to be solved (typically a simplex). It will not be detailed here since this first phase of the algorithm is not necessary with the hierarchical formulation.

A.3 Step length

At each iteration i, the algorithm computes the optimum χ ( * i) of the eQP associated to the current active set. It then moves the current χ (i) toward the eQP optimum:

χ (i+1) = χ (i) + τ (χ ( * i) -χ (i) ) (80)
If any constraint is reached during the move, then τ is chosen to saturate the most constraining constraint, which is added to the active set S i . The algorithm starts a new iteration.

A.4 Positivity of the multipliers

Otherwise, a full step τ = 1 is performed. In that case, χ (i) is optimal if the active set is optimal, which is true if the Lagrange multipliers are all nonnegative (this is one of the Karush-Kuhn-Tucker iQP optimality conditions [START_REF] Boyd | Convex Optimization[END_REF]).

If any multiplier is negative, the current active set is suboptimal and the corresponding constraint needlessly prevents to come closer to the optimum. The constraint corresponding to the multiplier with lowest value is removed from the active set S (i) and the algorithm starts a new iteration.

Algorithm 6 Classical active search

1: Input: χ (0) s.t. Cχ (0) ≤ d 2: Output: χ * minimizing (78) 3: S 0 = {k s.t. C k χ (0) = d k } 4: χ, S := χ (0) , S 0 5: repeat 6:
--Compute eQP optimum--7:

χ * = C + S d S + Z C (AZ C ) + (b -AC + S d S )
8:

--Compute the step length--

9: ∀i / ∈ S, τ i = Ciχ-di Ci(χ-χ * )
10:

τ, cst = min{1, τ i } 11:

χ := χ + τ (χ * -χ) 12:
--If necessary, increase the active set--13:

if τ < 1 then --If necessary, decrease the active set--

18: λ = -C +T S A T (Aχ -b) 19: ν, cst = min{λ} 20:
if ν < 0 then 21:

S := S \ {cst}

22:

end if 23: until τ = 1 and ν ≥ 0 24: return χ * := χ

A.5 Algorithm termination and convergence

Finally, the loops ends when both τ = 1 and all the multipliers components are nonnegative. The convergence of the loop in a finite time can be proven by showing the strict decrease of the cost function at each iteration [START_REF] Boyd | Convex Optimization[END_REF]].

B A simple example of active search

Fig. 2 gives an example of execution of Alg. 3 in 2 dimensions. The problem to solve is composed of two levels. The first (strict) level is:

1a : x 10 -y ≤ -0.55 (81)
1b :

xy ≤ 1.5 (82)

The second (relaxed) level is: 2a :

x ≥ 2.5 (83) 2b :

x + y ≥ 2 (84)

The algorithm starts at the initial point x (0) = [-0.5, 1.5], which satisfies the constraints 1a and 1b of the first level, but not the constraints 2a and 2b of the second level. All constraints are inactive. For each step of the algorithm, we give the optimum of the eHQP x ( * i) , the step, the change in the active set and the active set at the end of the iteration S (i) .

• computation of x (1) : we have x ( * 0) = [0, 0] but 1a prevents a full step and is activated. S (1) = {1a}

• x (2) : x ( * 1) is the projection of [0, 0] on 1a. A full step is taken, but the current point x is still outside of 2a and 2b. Both could be activated. 2b is chosen (arbitrarily, due to the constraint order). S (2) = {1a, 2b}

• x (3) : x ( * 2) is the intersection of the two active constraints. A full step is taken, but x still violates 2a, which is activated. S (3) = {1a, 2a, 2b}.

From this point, all the constraints are satisfied or active.

• x (4) : x ( * 3) is the point on the boundary of 1a at the least-square distance of 2a and 2b. A full step is taken and no constraint is activated. The Lagrangian of the first level is null since x is on 1a. The Lagragian of the second level corresponding to 2b (i.e. w * 2 ) is negative: 2b is deactivated. S (4) = {1a, 2a}

• x (5) : x ( * 4) is at the intersection of 1a and 2a, but 1b prevents to perform a full step and is activated. S (5) = {1a, 1b, 2a}

• x (6) : we have x ( * 5) = x (5) . A full step of length 0 is taken. The Lagrange multiplier is computed, and is negative on the component corresponding to 1a: the constraint is deactivated. S (6) = {1b, 2a}.

• x (7) : x ( * 6) is the intersection of 1b and 2a. A full step is taken and all Lagrange multipliers are positive: the optimum is found.

Hierarchical Quadratic Programming Part 2: Implementation and Experimentations Pierre-Brice Wieber

INRIA Grenoble St Ismier, France

Abstract Hierarchical least-square optimization is often used in robotics to inverse a direct function when multiple incompatible objectives are involved. Typical examples are inverse kinematics or dynamics. The objectives can be given as equalities to be satisfied (e.g. point-to-point task) or as areas of satisfaction (e.g. the joint range). This two-part paper proposes a complete solution to resolve multiple least-square quadratic problems of both equality and inequality constraints ordered into a strict hierarchy. Our method is able to solve a hierarchy of only equalities ten time faster than the classical method and can consider inequalities at any level while running at the typical control frequency on whole-body size problems. This generic solver is used to resolve the redundancy of humanoid robots while generating complex movements in constrained environment.

In the first part of the paper, we proposed a dedicated numerical solver to solve equalityonly and inequality hierarchical least-square quadratic problems. In this second part, we detail the implementation of the solver and its application to inverse kinematics. In particular, we explicit the solver complexity and prove the continuity and the stability of the resulting control laws. Finally, we experimentally demonstrate its efficiency in the context of generating robot motions.

Introduction

Dealing with multiple contradictory objectives is a classical situation with redundant robots. In that case, imposing a hierarchy between the objectives can be an elegant solution to handle the conflicts while ensuring a safe behavior, by setting important objectives at the top of the hierarchy. A typical situtation is the design of a control law on a humanoid robot: manipulation tasks should be perfomed only within the stability region of the robot, while postural or observation tasks can be only partially realized (see Fig. 1). Hierarchy of constraints expressed as linear equalities has been addressed early [Liégeois, 1977, Hanafusa et al., 1981, Siciliano and Slotine, 1991]. Inequality-based objectives were first taken into account at the lowest-priority level using potential fields [Khatib, 1986]. However, some top-priority objectives in robotics are often expressed as inequalities (e.g. joint limits or obstacles avoidance).

In [Kanoun et al., 2011], it was proposed to build a hierarchy of equalities and inequalities using a cascade of least-square quadratic problems (QP). However, the computation cost of the proposed method prevented any application for on-board robot control. A cheaper solution also based on QP cascades was proposed in [De Lasa et al., 2010] in a simplified case: inequalities are considered only at the first priority level, which is restrictive for simple case (e.g. Fig. 1-(a), the gaze is directly expressed as an inequality and set up as a third-priority objectives).

In the first part of the paper [Escande et al., 2012], we proposed a dedicated method to solve hierarchical problems without using a cascade of QP. For this purpose, we first proposed a matrix decomposition, named HCOD, dedicated to the hierarchical structure of the problem. Using this decomposition, the hierarchical problem with only equality constraints is solved in a single inversion. Based on the resolution of the equality problem, a dedicated activesearch algorithm was proposed, that solves the general problem while avoiding the large number of iterations typically encoutered when using a cascade of solvers. Moreover, the intermediary theoretical study emphasizes the underlying hierarchical structure of the problem and allows a better understanding that could be used for automatic supervision or automatic task sequencing.

The contributions and organization of this second part are the followings:

• we give the method to compute the HCOD and to modify it when the active set increases or decreases in Sec. 3.

• we compute the complexity of the hierachical solvers and prove the continuity of the solver optimum and the stability of the subsequent inverse-kinematics based control law in Sec. 4.

• we experimentally show in Sec. 5 the solver efficiency and its use in the robotics context to generate whole-body motions of a humanoid robot.

Background

In the first part of the paper, we proposed a solution to solve the following hierarchical problem: lex min

x,w1...wp { w 1 , w 2 , . . . , w p } .

(1)

subject to ∀k = 1..p, A k x ≤ b k + w k
where p is the number of levels of the hierarchy, and for any k = 1..p, A k and b k define the linear objectives to be satisfied at best in the least-square sense. This problem is formally defined by a cascade of p QP: min

x k ,w k w k (2) subject to A k x k ≤ b k + w k (3) A k-1 x k ≤ b k-1 + w * k-1 (4)
where A k-1 , b k-1 and w * k-1 are the matrix and vectors composed of the stacked quantities of levels 1 to k -1 (by convention, they are empty matrix and vectors for k -1 = 0). w * k-1 is the fixed value obtained from the QP of level k -11 . To solve this hierarchical least-square quadratic problem (HQP), it was previously proposed in [Kanoun et al., 2011, De Lasa et al., 2010] to use the cascade formulation by iteratively calling p times a QP solver. Such a cascade execution has some bad properties that are experimentally highlighted in Sec. 5. Instead, we proposed in the first part a hierarchical solver that is quickly recalled below.

Equality hierachical quadratic program

First, a subproblem of the HQP composed only of equality (eHQP) is solved (see the details in [Escande et al., 2012], Sec. 3).

The resolution of an eHQP is classical in inverse kinematics and dynamics, and was first formalized in [Siciliano and Slotine, 1991]. In the first part, we proposed a theoretical study that emphasizes the underlying mathematical structures and accelerates the resolution of this problem. We also gave a solution to compute the optimum of the dual problem. This method is based on the hierarchical complete orthogonal decomposition of the matrix A k (see [Escande et al., 2012], Sec. 3.3):

A k-1 A k = W k-1 0 0 0 V k U k   H k-1 0 0 N k 0 0 M k L k 0   [Y k-1 Y k Z k ] T = W k H k 0 Y k Z k T = W k H k Y T k (5)
where

W k = V k U k , Z k is a null-space basis of A k , L k is full rank lower triangular and W k , L k , Y k Z k is the complete orthogonal decomposition of A k Z k-1 : A k Z k-1 Z T k-1 = W k 0 0 L k 0 Y k Z k T (6)
The number of rows of A k is denoted by m k . The rank of

A k Z k-1 is denoted r k ≤ m k . The rank of A k is denoted r k = k j=1 r j .
Based on this decomposition, the hierarchical inverse of A k is defined by (see [Escande et al., 2012], Sec. 3.4):

A ‡ p = Y p H ‡ k W T p (7)
where H ‡ k is defined by the following recurrence:

H ‡ k = Y p H ‡ k-1 0 0 -L -1 k M k H ‡ k-1 0 L -1 k W T p (8)
The optimum of the eHQP is then simply:

x * p = A ‡ p b p (9)
Or, using again the HCOD, x * p = Y p y * p , with

y * k = y * k-1 y * k (10) and y * k = L -1 k (U T k b k -M k y * k-1
). The associated optimal w * k can be simply computed using the difference

w * k = A k x * p -b k .
A less expensive reformulation can also be expressed using the HCOD. The computation of the primal optimum is recalled in Alg. 1 of the first part of the paper2 .

For a given level k, the Lagrange multiplier associated with (4) is denoted by λ k . The multiplier associated to (3) is simply w k . Then, λ k can be computed using the hierarchical inverse by (see [Escande et al., 2012], Sec. 3.5):

λ k = -A ‡T k-1 A T k w * k (11)
Once more, a cheaper reformulation can be obtained using the HCOD and is summarized in Alg. 2. All the multipliers can be summarized under a pseudo matrix structure:

Λ p =        w * 1 1 λ 2 . . . 1 λ p-1 1 λ p w * 2 . . . 2 λ p-1 2 λ p . . . . . . . . . w * p-1 p-1 λ p w * p        (12)
where j λ k (j < k) denotes the components of the multipliers λ k of the level k for the constraints of level j, and the empty spaces for j > k express the absence of multipliers on above levels. On the contrary to the primal variable x * p , there is no direct formulation to compute the whole dual variable Λ p . Consequently, the multipliers of each level have to be computed iteratively.

Hierarchical active search

The inequality hierarchical program (iHQP by opposition to the eHQP) defined in (1) is solved using an active-search algorithm: if we know the set of inequality constraints that hold as equalities at the optimum, then the resolution of the iHQP simply involves the resolution of the eHQP composed of the active set of constraints. The active-search algorithm iteratively searches the optimal active set. It starts with an arbitrary initial guess (an empty set, if no better guess is available). At each iteration, it computes the optima of the primal and dual eHQP associated to the current active set, and deduces one constraint to be added or removed to/from the active set.

In the first part of the paper ( [Escande et al., 2012], Sec. 4.3), we proposed in Alg. 5 an adaptation of the classical active-search algorithm that solves the whole hierarchical problem inside a unified loop by searching for the active set of all the levels of the hierarchy at the same time.

Decomposition computation and modification

In this section, we present a complete solution to compute the HCOD of a matrix A. This solution is based on iterative orthogonal transformations, the Householder reflections and the Givens rotations (recalled in App. B). The algorithm presented in the previous sections uses the HCOD to compute the primal x and dual λ optima at each active-search iteration. However, the decomposition is costly and should be avoided if possible. It has to be computed once for the initial active set at the first iteration. At the next iterations when one element of the active set is added or removed, the initial HCOD only needs to be updated to fit with the current active set. While the computation of the HCOD is cubic in the matrix size, the update is quadratic and keeps a low computation cost for the active search even when several iterations are needed. A complete solution to apply such an update is presented in the two last subsections.

Computation of the HCOD

Right basis Y

The HCOD is computed iteratively for each level k. We suppose that the expression of A k in the basis Y k-1 Z k-1 is computed. A rank-revealing LQ decomposition of A k Z k-1 is first performed: the row of largest norm of A k Z k-1 is selected and permuted at the top of the matrix by a permutation matrix Π 1 . The Householder reflection Q 1 nullifying the tail of this row is built using (47) and applied to the m k -1 last rows of A k Z k-1 using (46). The result of this first reflection can be written:

A k Z k-1 = Π 1      l 1 0 . . . 0 × K 1 . . . ×      Q T 1 (13)
The reflection Q 1 is also applied to the levels A k+1 to A p . The same process is iteratively applied to the remaining matrix K 1 , until all the rows of the remaining matrix are null. The result can be written:

A k Z k-1 = Π        L R 0 . . . 0 . . . . . . L N 0 . . . 0        Q T (14) with Π = r k i=1 Π i , Q = r k i=1 Q i and r k the rank of A k Z k-1 (i.e. the size of L R ). This decomposition of A k Z k-1
is available in many classical linear algebra software by applying a column-pivoting QR decomposition on the transpose (e.g. QR in matlab, xGE-QPF in LAPACK, ColPivHouseholderQr in Eigen). Q is a transformation of Z k-1 that reveals the rank of level k and gives the component Y k of the right basis:

Y k Z k = Z k-1 Q (15)
The matrix Q is not expressed explicitly but as a structured product of r k Householder reflections Q i , whose vectors v i are of decreasing support size. Finally, the obtained basis Y = Y p Z p is expressed as a structured matrix of r p Householder reflections. Each reflection i corresponds to a vector v whose support size is ni and does not modify the i first columns of A p . If needed, the coefficients of the matrix Y can be explicitly computed by applying the reflections on the identity matrix I n .

Left bases W k

For each stage k, the matrix L N has now to be nullified by applying a transformation on the left side of the decomposition. We can suppose that L N has only one row (the same procedure is then applied for each rows of L N ). Each component i of the row L N starting from the last one is nullified by applying a Givens rotation pivoting around the row i of L R . The result of the i th rotation can be written:

Π W (i)              × 0 0 . . . . . . × • • • l ri . . . . . . . . . . . . × • • • × • • • × × • • • l ni 0 • • • 0 • • • 0              Q T
where the double arrow represent the next Givens rotation to be applied. The matrix W k is built recursively using the following series of Givens rotations:

W (i) = W (i-1) G[r k -i, r k + 1, θ i ] (16)
where θ i is chosen to nullify l ni , i.e. the i th column starting from the right of the matrix W (i-1) T L R L N . The recursion starts with W (0) = Π and stops with

W k = W (r k ) .
The left part A k Y k has been already computed when applying the Householder reflections Q i on A k . The matrices M k and N k are computed by applying the r k Givens rotations to A k Y k . Finally, the rows of the matrix corresponding to L R and L N can be exchanged by applying a last permutation. The decomposition at level k is then written:

A k = W k N k 0 0 M k L k 0 Y (17)
with W k a product of Givens rotations and permutations and Y a product of Householder reflections.

Decomposition update

We now present the HCOD update, i.e. how to add one constraint at level k. The next section will present the removal of a constraint.

Update properties

We consider the HCOD W p ,H p ,Y of the matrix A p . One row A up is added in the level k. The first part of the update consists in finding a proper decomposition of this matrix for level k, mainly by applying a series of Givens rotations Y up on the right. The second part consists in propagating Y up on the levels j > k.

The stack of A k and A up can be written:

A k A up = W k 0 0 1     N k M k 0 0 • • • 0 • • • 0 L k 0 • • • 0 • • • 0 A up Y 0 • • • 0     Y T
(18) The update procedure depends on the number of 0 of the tail of the new row.

The insertion of A up in A k can increase the rank of the HCOD (i.e. r p ) of 1 or let the rank unchanged. Similarly, the rank of level k can increase of 1 or stay unchanged. Finally, the ranks of the upper levels can only decrease or stay unchanged since they are not augmented by any rows. In conclusion, at most one matrix from k + 1 to p can lose at most one rank, all the other rank being kept unchanged. The level that loses one rank is denoted k. If the updated matrix k does not change its rank, all the following rank will stay constant too. This case can be seen as k = k.

The rank loss means that A up is linked with A k (i.e. A up is a linear combination of the rows of A k). Consequently, A up Y T is null on the columns corresponding to levels k to p and its support equal to A up Y k. The row of level k that has the same number of nonzero elements (abusively named the rank of the decomposition row) as A up Y is denoted by r.

Case 1. A up is linked with k

The simplest case to update is if there is at least as much 0 in the tail of the new row than on the last row of H k :

A k A up = W k 0 0 1     N k M k 0 0 0 L k 0 0 A up Y 0 0     Y T (19)
This means that the new row is linked with A k . The decomposition is completed by nullifying the part of A up Y k below L k using a left basis W up as explained in Section 3.1.2. As a result, the decomposition of level k can be written:

A k A up = W k 0 0 1 W up     N k M k 0 0 0 L k 0 0 N up 0 0 0     Y T (20) with N up = W T
up A up Y and W up computed with ( 16). The update is completed for level k by reordering the rows of H k and the corresponding columns of W k . Since there is no effect on the level j > k, the update of the whole HCOD is completed.

Case 2: A up is not linked with A k

The first step of the update is to turn A up Y into a new row of H k . This can be done directly by applying a set of Givens rotations on the right to nullify the elements starting from the last nonzero one using the previous element as a pivot.

Y (i) up = Y (i-1) up G[r -i -1, r -i, θ i ] ( 21
)
where θ i is chosen to nullify the corresponding element of A up Y Y . The updated matrix at level k can then be written:

A k A up = W k 0 0 1     N k M k 0 0 0 L k 0 0 A up Y X 0     Y T up Y T (22)
where X is the edge of the new triangle formed by L k and the added line. The left part of the new row (i.e. A up Y k-1 ) is not modified by Y up . X is nonzero (otherwise, it corresponds to the case 1). This decomposition then gives the update for level k (i.e. of W k , M k , N k and L k ). The update can now be propagated to the next levels.

Propagation of Y up

The propagation of Y up and the consequent modifications of the levels k + 1 to p are applied itera-tively from level k + 1. For each row with a 0 tail h

= [× • • • × 0 0 • • • 0 ]
, the multiplication with Y up keeps all the 0 of the tail, except the first one, due to (21):

hY up = [× • • • × X ′ 0 • • • 0 ] (23)
where X ′ is at the position of the first 0 of the tail of h. X ′ is nonzero in the general case but is zero if the rank of h is greater than r, once more due to (21). Consequently, the shape of the decomposition at level i after application of Y up is:

A i = W i           N i M i ν 1 0 0 . . . 0 0 ν m-r 0 0 L i d 1 d r . . . . . . 0 0           Y T up Y T
with (d 1 , ..., d r ) a set of r i component forming a upper bidiagonal on L i and (ν 1 , ..., ν m-r ) a column vector of m ir i components. For all lines j with a rank greater or equal to r, d j is zero. It is possible to distinguish three cases, whether the rank loss happens before, after or at the current level i.

Case 2.1: rank loss after the level i

In the first case, the HCOD for levels i-1 and i looks like:

                 N i-1 M i-1 0 L i-1 0 0 N i M i ν 1 0 0 . . . 0 0 ν m-r 0 0 L i d 1 d r . . . . . . 0 0                  (24)
with all the d j nonzero. From this structure, the update of the decomposition of level i is directly obtained.

Case 2.2: rank loss at the current level i

The structure of the decomposition is similar to the previous one, but there is l zero components in the begining of d j : the row l of H i has the same support size as A up Y . By reordering L i to put the row l at the bottom row and reordering accordingly W i , the decomposition can be put under the form ( 19).

Following, the last row can be nullified by the set of Givens rotations W up , as described in Sec. 3.2.2.

Case 2.3: rank loss before level i

In that case, the total rank of the HCOD at the previous level r i-1 did not change, and the levels i -1 and i have the following structure:

                N i-1 M i-1 0 L i-1 0 0 N i M i ν 1 0 0 . . . 0 0 ν m-r 0 0 L i d 1 d r . . . . . . 0 0                 (25)
However, since the rank loss occurred in one of the previous levels, the Y up does not modify the columns corresponding to L i . Following, the ν j and d j are zero, and the given decomposition is directly the update of the current level.

Decomposition downdate

When one constraint is deactivated, the corresponding row of A p is removed. The modification of the HCOD to correspond to the new A p is called downdate (by opposition to the update).

Removing one row of H k

Without loss of generality, we can consider the case where the last row of the level k should be removed (a permutation is first applied otherwise). The general idea is to find a rotation W dw so that W k W dw is unitary on the row to be removed:

W k W dw =      0 W A . . . 0 0 • • • 0 1      (26)
The rotation W dw can be built as a product of Givens rotations:

W dw = m k -1 i=1 G[i + 1, i, θ i ] ( 27
)
where θ i is chosen to nullify the i th component of the row of W to be removed. The result on H k can be written:

A k = W k W dw               N i M i 0 0 0 . . . 0 0 0 0 0 d 1 0 0 L i d r . . . . . . 0 0 M dw L dw 0 0               Y T
The last row of H k and the last row and column of W k are then removed. If all the d 1 ... d r are nonzero, then the obtained form is a proper HCOD. Otherwise, the resulting L k is upper Hessenberg and can be corrected by using a serie Y dw of Givens rotation applied on the right side, as in the update case.

Propagation of Y dw

We now consider the level i > k. After application of Y dw , the HCOD of levels i -1 and i looks like:

               N i-1 M i-1 0 0 0 L i-1 N i ν 1 . . . ν m-r M i 0 0 L i 0                (28)
• Case 2.1: If all the ν 1 . . . ν m-r are null, then the triangle part of H i is upper Hessenberg. The process already explained for H k is applied on H i and the obtained Y dw is propagated to H i+1 .

• Case 2.2: Otherwise, we can suppose without loss of generality that ν m-r is nonzero (the corresponding row is linked to the row removed from level k).

The rank of level i is increased by one. The decomposition is finalized by simply nullifying the nonzero ν 1 . . . ν m-r-1 by applying Givens rotations on the left of H i .

• Case 2.3: after Case 2.2, the propagation of Y dw on the levels i + 1 and further will not modify the corresponding L matrix, which concludes the HCOD modification.

A complete implementation of the decomposition and the updates, along with HQP resolutions, is proposed in App. D.

Structured versus explicit matrix

The cost of a single elementary transformation is much lower when it is kept as a structured matrix. Then, the explicit computation Y is not needed. However, when n Householder reflections are involved, the cost of a multiplication by Y is the same for an explicit or a structured expression. This is typically the case after a full HCOD, since the problem is often fully constrained and thus involves n reflections. Then, after any update or downdate, Y also implies a set of Givens rotations. To avoid an increase of the cost of Y along the active-set iterations, we transform in practice Y to an explicit matrix at the end of the first HCOD.

Similarly, the W k are given as a set of Givens rotation. However, some coefficients of W k need to be explicitly computed during a downdate (in ( 26)). For simplicity, we also compute the explicit form of W k , except when the level k is full rank (W k is then the identity).

Implementation considerations

In this section, we give some results and techniques about the effective implementation of the solver. We first characterize the computation cost of both the eHQP and iHQP solvers. When the solver is used to compute the robot control, one instance of the problem is solved at each control cycle. In that case, we prove under some conditions the continuity of the control law when some constraints become active. This continuity can be used to reduce the computation cost. Finally, we prove the overall stability of the control scheme when the solver is used to execute an inverse kinematics.

Complexity

We roughly characterize the cost of the HCOD decomposition, of the eHQP resolution knowing the HCOD and of the whole iHQP algorithm.

Cost of a HCOD

We first consider a matrix A of size m×n. The LQ decomposition of A = LQ requires min(m,n) i=1

(ni) ≈ nm Givens rotations, each of them being applied on A and Q. The cost to obtain L is then nm 2 elementary operations, while the cost for Q is mn 2 .

A COD V U 0 0 L 0 Y Z T starts with a LQ before applying a QL on the obtained AY . The cost of the QL is (mr)r 2 to obtain L and (mr)m 2 to obtain W = V U . The total cost of the COD is roughly n 2 m + nm 2 + (mr)m 2 . When m is much smaller than n (typically for A 1 ), the cost is in O(mn 2 ) while when m and n have the same magnitude order (typically for A p ), the cost is in O(2n 3 ). We now consider the set of p matrices A k , each of them being of size m k ×n, and of rank in the hierarchy

r k = r k -r k-1 . The size of A p is m = m k .
For simplicity, we consider that all the decompositions are computed using a series of Givens rotations, and that all the basis are computed explicitly into a dense format.

The HCOD can be computed by a serie of COD. For each COD, the coefficients of the intermediary bases Z k are not necessary: only the A k+1 Z k are needed. The Y k and Z k are kept as product of elementary transformations. The number of operations to get the right part of the decomposition Y is the same as for getting the right part of decomposition of the COD. The QL decompositions W k are then made for each level k independently, and the cost is each time the same as for the QL of the COD of A k . The total cost is then:

c HCOD = n 2 m + nm 2 + p k=1 (m k -r k )m 2 k (29)
The cost is less than the COD of A p but more than its QR. Of course, if all the matrices are full-row rank, the three costs are the same.

Cost of the eHQP

We consider now the two algorithms 1 and 2. Since w * p can be computed in both, we remove the redundant computations from Alg. 1.

The computation involved by the primal optimum amounts to the forward substitution of the stacked U k b k by the triangular matrix formed of the M k and L k . The cost is m k r k for computing the U k b k and then r 2 p for the forward substitution. The total cost is roughly r 2 p . The computation of the multipliers of level k is similar: it is the backward substitution of the ρ, whose own cost is negligible with respect to the cost of the substitution. The cost is then r 2 k . The cost to compute Λ p is then r 2 k ≈ r 3 p which is similar to the cost of the whole HCOD.

In conclusion, if the proposed method is used to compute only the primal optimum of a eHQP, the cost is similar to the cost of the inversion of a full-rank system of the same size using a QR decomposition and a forward substitution.

If both primal and dual optima are needed (for example in the active search), the computation of the multipliers Λ p is costly and should be parsimoniously realized.

Cost of the HQP active search

Two supplementary operations are executed during the active search: the computation of the step length and the HCOD update.

Consider first the update. A new row is added at level k, with r nonzero elements in A up Y p . There is then r -r k Givens rotation to execute and propagate, first on the least-priority levels H k+1 . . . H p (for a cost of (rr k ) p j=k+1 m j ) and, second on the basis Y (for a cost of (rr k )n). If r < r p , there is also one level j that becomes rank-deficient and for which an update of W j is needed, for a cost of 2r 2 j . The total cost is then in O(n 2 + mn).

Using the same reasoning, the cost of a downdate is also O(n 2 + mn).

Finally, denoting by s k ≥ m k the total number of constraints of level k (i.e. active and inactive), the step length computation for each level requires the multiplication of the rows of

A k for the s k -m k inactive constraints. The cost is k n(s k -m k ) = n(s -m) with s = k s k .
The cost of the active search depends of course on the number of iterations in the inside loop (the number of iterations of the outside loop being bounded). If during an iteration a constraint is activated, the iteration implies the computation of the primal, of the step and an update. If the iteration is concluded with a deactivation, it implies the primal, the step, the dual and the update. Finally, due to the outer loop of Alg. 5 there are p iterations that do not activate nor deactivate any constraints. We denote by N U and N D the number of activations and deactivations of the algorithm. The total cost is easily deduced from the above list and Table 1.

If we suppose additionally that the hierarchical problem constraints the whole parameter space (m = n) and if we smooth the differences between the level for the dual computation and the updates (approximate costs are given on the third column of Table 1), the total cost is:

c asearch = 2n 3 + (3N U + 4N D + 2p)n 2 + (N U + N D + p)n(s -n)
In particular, the minimum cost of the active search operation cost approx.

QR is obtained for N U = N D = 0:

nm 2 + n 2 m 2n 3 COD n 2 m + nm 2 + (m -r)m 2 2n 3 HCOD n 2 m+nm 2 + (m k -r k )m 2 k 2n 3 Primal x * p r 2 p n 2 Dual w * k , λ k r 2 k n 2 Dual Λ p p k=1 r 2 k pn 2 Update n 2 + mn 2n 2 Step τ n(s -m) n(s-n)
c asearch = 2n 3 + np(n + s)
In that last cost, compared to a standard QP, the hierarchy brings the p factor in the last term. This is mainly due to the bidimensional structure of the multipliers Λ p .

Without surprise, the cost of the active-search strongly depends on the number of activations and deactivations N U + N D that are needed to reach the optimal active set. If a good approximation of the optimal active set is known, then the total cost of the active search can be significantly decreased. In particular, if a similar problem has already been solved, then the obtained optimal active set can be reused for the current problem. This reuse of a part of the solution of a slightly different problem is called warm start and is detailed in the following three sections. We first define this notion of similar problem.

Parametric optimization

We now consider that HQP, that varies continuously with respect to a given parameter t:

lex min x,w1...wp { w 1 , w 2 , . . . , w p } . ( 30
) subject to ∀k = 1 : p, A k (t)x ≤ b k (t) + w j
with A k (t) and b k (t) continuous function of a real parameter t ∈ R. This problem is denoted by HQP(t).

We study the continuity of the two functions:

O : t → O(t) = {x, ∀x ′ , A p x A p x ′ } (31) x * : t → x * (t) = min x∈O(t) { x } (32)
where b 1 b 2 denotes the lexicographic order corresponding to the hierarchy. O(t) is the optimal set of the HQP(t) and x * (t) is the least-square element of O(t) (unique since O(t) is convex).

Continuity of the parametric optimum

First, the eHQP optimum is known to be continuous outside of the singular regions. This is formalized by the following result. The functions giving the rank of the projected matrices are denoted by:

r k : t → r k (t) = rank(A k (t)) -rank(A k-1 (t)) (33)
Theorem 4.1. Consider a eHQP of the form (30).

For a given t 0 , if the maps r 1 ...r p are constant on a neighborhood of t 0 , then O(t) and x * (t) are continuous at t 0 .

Proof. The result is straightforward using the fact that the pseudo-inverse map A → A + is continuous inside the set of constant rank of A [Ben-Israel and Greville, 2003].

When the r k (t) are not constant, the solution maps are not continuous in general. The only case where the continuity is not ensured is when passing inside a singular region and only at the region border. Inside the singularity, the optimum remains continuous.

Consider now the full problem (30) with inequalities. It is possible to show that, apart from these discontinuities due to the singularity of the eHQP, the continuity of the optimum is ensured. S * (t) denotes the optimal active set of HQP(t) corresponding to x * (t). For a given active set S, the eHQP associated to (30) is denoted by eHQP(t, S) and the optimum of this eHQP is denoted by x * S (t). We then have the following result:

Theorem 4.2. At a given t 0 , if t → x * S * t 0 (t) is con- tinuous, then x * (t) is continuous.
Proof. If the optimal active set is constant in a neighborhood of t 0 , the result is straightforward using Th. 4.1. We then consider the case where the active set changes on t 0 : ∀t > t 0 , S(t) = S(t 0 ). We suppose that the active set of level k is increased by one constraint A up x ≤ b up in t > t 0 (the case with multiple activations or deactivations follows naturally). Since (A up , b up ) is active for any t > t 0 of a neighborhood, it means the optimum lies on this constraint at t 0 : A up x * p (t 0 ) = b up . Both active sets S(t 0 ) and S + = S(t 0 ) {(A up , b up )} give the same x * p . Using the continuity of the eHQP associated to S + the continuity of x * (t) toward x * (t 0 ) is straightforward.

The continuity of the solution before and after the update can be understood by looking at the structure of y * p in the four cases of Sec.3.2: the HCOD update may keep r k constant or increase it, and in the second case, may decrease the rank of level j > k or increase the total rank r p .

In the first case, only the continuity of

y * k is questionable.
It is ensured by the continuity of the partitioned generalized inverse [START_REF] Ben-Israel | Generalized inverses: theory and applications[END_REF]:

A C b d → d→CA + b A + b (34)
for any A, b, C. The continuity of the y * j , j > k follows, using recursively the continuity of y * j-1 and the continuity of the eHQP.

If both r k and r p increase, the update adds a new column Y up in Y k . This vector belongs to the null space of the HCOD at t 0 : Y T up Y p (t 0 ) = 0. This case is then equivalent to considering that (A up , b up ) was added as a new least-priority level p + 1 of the hierarchy. The y * p is continuous using Th. 4.1. And y * p+1 is continuously evolving from 0 using (34). Finally, the key case is when the rank increase at level k and causes a rank loss at level j > k. It means that Y up belongs to Y j (t 0 ). In that case, the DOF corresponding to Y up was allocated to the level j before t 0 . At t 0 , the constraint A up x ≤ b up is reached. The DOF is then reallocated to the level k. The reallocation is performed when the optimum is exactly on the constraint, thus causing no discontinuity. This case is depicted in Fig. 2.

Warm start and real-time implementation

On-board a robot, the solver is used at discretized times t,t+1,... Thanks to the continuity, the optimum of time t nearly satisfies the feasible constraints of time t + 1. A good initial guess for the active set of time t + 1 is thus the optimal active set of time t. In most of the cases, this is enough to reach the optimum in only one iteration. From time to time, the active set changes: in most cases few additional activations and deactivations are sufficient. However, one cannot guarantee the number of necessary iterations to reach the optimum. In particular, a minor modification of one optimum can trigger a cascade of activationsdeactivations in pathological cases. In these pathological cases, it is possible to force the solver to stop after an arbitrary number N of iterations. It is then not ensured that the optimum is reached and three sub-optimal cases arise depending on what happened during the last iteration: if τ was less than 1 (case of Alg. 4#17), it means that all the feasible constraints are satisfied, but the optimum of the corresponding eHQP is not reached yet. If an update was triggered with τ = 1 (case of Alg. 4#11), it means that some of the constraints might still be unsatisfied and inactive. In that case, the residue of these constraints could be improved by activating them using some more iterations. Lastly, if a downdate was triggered, the eHQP optimum is reached but not the optimal active set. With more iterations, some unnecessary constraints could be relaxed, that would free some DOF to improve the solution.

Thanks to the continuity property, it is possible to show that the obtained final point x (N ) is at a distance to the optimum x * proportional to the sampling of the control. If the optimal active set was not reached at t + 1, the search will restart at t + 2 from the intermediary active set obtained after the N iterations of time t + 1.

By using the previous active set as a warm start and bounding the number of iterations, the solver will find the optimum in real time in most of the case, and otherwise will respect the real-time constraint by spreading the search of the optimal active set over several iterations.

Stability

We consider now the practical case of the inverse kinematics: the robot velocity q is computed as the optimal solution of a HQP, where the constraints are defined by a set of p tasks:

ėk = J k q (35) ∀k ∈ S I , ėk ≤ b k (36) ∀k ∈ S E , ėk = ė * k (37)
where S I S E is a partition of the set {1 . . . p} of the p first integers: S I are the task levels that are defined by a limit b k and S E are the task constraints to follow a given velocity ė * k . When the task function e k is an error between a reference and a current sensor value, the task reference ė * k is often given to drive the error to 0:

ė * k = -κ k e k (38)
where κ k > 0 is a gain used to tune the convergence speed. Finally, we suppose that 0 is an acceptable solution for all the tasks of S I , i.e. ∀k ∈ S i , b k > 0.

The robot input q is then computed as the result of a HQP: lex min q,w1...wp { w 1 , w 2 , . . . , w p } .

subject to (35),( 36), (37). In this section, we briefly prove that the use of a HQP keeps the same proper-ties of control stability than in other classical inversekinematics approaches. We define by e E the stack of all equality constraints:

e E =    e k1 . . . e kE    (40)
for S E = {k 1 . . . k E }, and by J E the associated jacobian.

Theorem 4.3. The control law defined by (39) is stable in the sense of Lyapunov. It is asymptotically stable iff J E is full row rank and none of the equalityconstraint levels of the HCOD are rank deficient.

The idea of this theorem is illustrated in Fig. 2: if the optimum is inside the bounds (36), then the close-loop inverse-kinematics eHQP is known to be stable [Antonelli, 2009]. Otherwise, the system will smoothly stop on the boundary. The inequality prevents the robot to come closer to the objective but does not imply an unstable behavior. The stability is kept even if the boundaries prevent the system to be asymptotically stable. The complete proof is given in App. C.

If the robot is not inside the confidence area (then some b k are not positive), then the corresponding task components can be seen as equalities that bring the robot inside the area, which ensures the stability during the transition phase.

Experiments

We first present in Sec. 5.1 the computation times obtained with both eHQP and iHQP out of any robotics application. Then the iHQP solver is used in the following sections to perform an inverse-kinematics control scheme on a humanoid robot.

Computation time

Equality-only HQP

We first experimentally check the computation time needed to compute the primal optimum of a eHQP using the three following methods: • M P : using iterative pseudoinversion and explicit projector P k , as proposed in [Siciliano and Slotine, 1991] (recalled in Eq. ( 15) of the first part)

• M Z : using iterative pseudoinversion and the basis of the null space Z k (recalled in Eq. ( 17) of the first part)

• M H : using the hierarchical inverse (9).

Since the pseudoinverses and projectors of M P are generally computed using a SVD [Baerlocher and Boulic, 2004] which is well known for being slower than a COD, the same M P is computed twice, once with a SVD and the second time with a COD. A set of eHQP problems is resolved. These solvers are run on a set of problems whose size parameters (number of columns, of rows and total rank) are the same but whose number p of levels of the hierarchy varies. The computation costs are presented in Fig. 3. For all methods, the cost increases for small p. The hierarchy induces very little extra cost, even for large p, and no significant cost at all when using our method. As expected, the SVD is very expensive compared to the COD. Apart from the additional cost due to the SVD, the total cost is similar for M P and M Z but is differently distributed: M P spends more time in the decomposition while M Z spends more time in computing Z. Finally, by working with y * directly and using only one basis Y for all the decomposition, M H is the most efficient. Most of the time is spent in the decomposition. Inverting L and translating y * p into x * are negligible. The method is approximately six times faster than the classical SVD-based M P (and up to ten times for small p).

Active search

Similarly, the hierarchical active search Alg. 5 is compared to the cascade of QP used in [Kanoun et al., 2011]. The results are shown in Figures 4 and 5. The computation cost increases with the number p of levels when using a cascade of QP, while it remains constant with Alg. 5 as shown in Fig. 4. This is due to an increase of the number of iterations of the active-search loops used in the cascade, as shown in Fig. 5. Indeed, the cascade activates a set of constraints at level k that may not be necessary at level k + 1 and activates it again at level k + 2. These back-and-forth decisions are avoided when considering all the levels at the same time. [Kanoun et al., 2011] or a our HQP solver (Alg. 5). A set of hierarchical problems is solved, whose size parameters are the same (n = 100, m = 150, r = 80) but whose number p of levels varies. The computation time is plotted wrt p. The time increases with the number of levels when using a cascade, while it is nearly constant with our solver. A little overhead appears for high p and is due to the computation of Λp. For p = 1, the cost are exactly the same.

Robotic setup

The solver has been experimentally tested to perform an inverse kinematics with the HRP-2 robot. The robot has 36 DOF whose first six are not actuated. When considering only the kinematics, a classical solution is to replace the underactuation by a contact constraint of equivalent size, typically constraining the six parameters of position and orientation of one contacting foot. The underactuation is then resolved while the dynamic of the robot is negligible. All the QP resolved in the following have a parameter size n = 36. The robot is controlled at 200Hz.

Two sets of tasks are considered. The first set aims at regulating to 0 an error depending on the robot configuration. The task function is then given by: e(q, Ω) = s(q)s * (Ω) (41) where the task function e depends on the robot configuration and other parameters of the universe Ω as an error between a current measurement s(q) and a desired value of it s * . The functions used in the following are the end-effectors position and orientation of the right hand e rh , left hand e lh and feet e rf , e lf .

Both feet are controlled together on the ground by stacking the two last tasks e f eet = (e rf , e lf ). The center of mass (COM) is controlled to a given point by e com . The orientation of one vector attached to the robot (for example, having the hand around a stick but able to rotate around the stick axis) is controlled by regulating the cross product:

e θ = u(q) × u * (Ω) ( 42
)
where u is the vector attached to the robot to be placed on u * . More details about these classical task functions can be found in [Sentis, 2007, Kanoun, 2009]. The regulation of the error is realized by the proportional correction (38). The linear constraint is finally:

J q = ė * - ∂e ∂Ω Ω ( 43
)
where J = ∂e ∂q is the robot task Jacobian. The second set of tasks defines a bound on a function of the robot configuration:

e l (Ω) ≤ e(q) ≤ e u (Ω) ( 44
)
This constraint is homogeneous to the configuration. The linear constraint is obtained by the first-order Taylor approximation: κ ∆t (e l (Ω)e(q)) ≤ J q ≤ κ ∆t (e u (Ω)e(q)) (45

)
where κ is the time horizon (in number of control iteration) used as a gain modifier [START_REF] Faverjon | A local based approach for path planning of manipulators with a high number of degrees of freedom[END_REF].

Several task functions e can be used to obtain various behaviors. The joint-limit task e jl bounds the robot configuration q by a set of fixed value. The task e supp keeps the COM in the support polygon. The field-of-view (FOV) task e f ov considers the projection of an object on the image plane and bounds it by the image border. The collision avoidance is enforced by the task e coll by imposing the distance between a body of the robot and an object to be positive. In that case, the pair of bodies and object to check has to be specified (no systematic collision checking was performed here, it should be considered in the future [START_REF] Stasse | Real-time (self)-collision avoidance task on a HRP-2 humanoid robot[END_REF]). Once more, details about these classical functions can be found in [Sentis, 2007, Kanoun, 2009].

Experiment A: grasping using conditional visual guidance

The robot has to grasp a point object while keeping its balance and respecting its joint limits. During the task, the robot tries to keep the object in its FOV and, if possible to keep its COM into a small 2cm-wide band inside its support polygon to obtain a well-balanced posture (e bal ). The task order is then e jl ≺ e f eet ≺ e supp ≺ e rh ≺ e f ov ≺ e bal . The robot motion is summarized by Figures 6 to 14. The ball is moved in four different places as shown in Fig. 6: first in front of the robot, in an easily reachable position; then far in the left, so that the robot has to bend to reach it; the ball is put back to the initial position before putting it to a far right position that is not reachable while keeping the COM inside the support polygon. At the first ball position, the two last tasks are respected: the ball is inside the FOV and the COM is in the central band. The second ball position is further away and requires to the robot to importantly bend to reach it. The COM cannot be kept inside the central band while satisfying all the other tasks. Relaxing the leastpriority COM task enables to satisfy e f ov and e rh . The higher-priority COM task is respected, that ensures the robot balance. The ball is then placed back in front of the robot: the COM comes back to the central band while all the other tasks are kept satisfied. The last position is unreachable while keeping the COM in the support polygon. All the tasks from the least-priority one are successively relaxed until the minimal distance to the ball is finally reached: at the final position, the COM is outside of the central band, on the border of the support polygon, and the ball is outside the FOV. This is a typical case of interest of the hierarchy: a proper behavior is ensured by the tasks having priority (balance, joint limits) while the optional objectives are satisfied at best.

The task sequence is given in Fig. 7. In the beginning of each motion sequence (when the ball is just moved), the visibility constraint (44) might be violated without the FOV task (45) being violated: the control is simply bringing the ball inside the FOV boundaries according to the task definition. The task becomes violated when (45) cannot be fulfilled. Details about the COM and FOV satisfaction are given in Figures 8 and9. At the beginning of the third motion sequence, the COM is outside of the central band. It is brought back to this zone after 0.2 seconds. Similarly, at the beginning of each sequence, the ball is outside of the FOV and is quickly brought back. At time T = 4s, the COM is at the centralband limit when several joints reach their limits (see Fig. 10). This reduces the available DOF for the grasp task, and following for e bal , which has to be relaxed: the COM leaves the central band. Similarly, at T = 9s, the COM is on the border of the band. The activation of some joint limits once more drives the COM outside of the central band. At T = 9.5s, some DOF of the grasp task e rh collapse because of a kinematic singularity. The reallocation of the DOF used by the least-priority tasks leads first the X coordinate of the COM to leave the central band, then the ball to leave the FOV. The COM quickly escapes the central band, until it finally reaches the second COM bound imposed by e supp . The limitation of the COM causes the violation of e rh : the robot then stops as close as possible to the ball. Some typical trajectories of the joints are shown in Fig. 10: the limits are always respected. The number of active constraints for all levels together (i.e. the size of the optimal active set) is displayed in Fig. 11. [Kanoun et al., 2011].

Finally, the computation cost is experimentally studied. We compare the HQP resolution using a cascade of QP as proposed in [Kanoun et al., 2011] (Fig. 12), with our method using an empty initial guess (Fig. 13) and using a warm start as proposed in Sec. 4.4 (Fig. 14). First, the number of iterations in the active search loop is much higher with a cascade of QP than using the proposed HQP solver. The number of iterations in the active search is very similar to the number of active constraints at the optimum, as can be seen by comparing Fig. 11 to Fig. 13-(top). Our solver barely needs any deactivation to find the optimal active set. This is not the case when using a cascade of QP: Fig. 12- (top) shows that approximately twice as many iterations are need to reach the optimal active set. As expected, the number of iterations is even lower using a proper warm start: in that case, the active search only iterates

Experiment B: opening a valve

The robot has to open a valve by manipulating a wheel. The motion is composed of two parts: the robot first manipulates the wheel using one hand, then rotates the wheel using both hands with successive re-grasps. During the motion, the robot has to avoid a block located on its left, and to keep its COM inside the support polygon. When grasping the wheel, the robot has to look at it; when rotating the wheel, it has to look at a pression meter located on its left. All the inequalities are considered at the top priority levels so that the HQP behavior can be compared to the solution proposed in [De Lasa et al., 2010]. The first movement (left-arm manipulation) is summarized in Figures 15 to 18 For the four inequality tasks, the number of active constraints during the motion are plotted for each levels. around the wheel axis is let free and only two degrees of rotation are controlled. The hierarchy is e jl ≺ e supp ≺ e f ov ≺ e coll ≺ e f eet ≺ e T lh ≺ e Rlh .

The number of active constraints for the four first levels is also shown in Fig. 16. The total number of active inequalities is given in Fig. 17. When the robot is on the left part of the wheel, the obstacle strongly constraints the robot, which causes an increase of the number of active constraints: two peaks appear for each loop of the wheel. The distances of the shoulder and elbow to the obstacle is given in Fig. 18. The arm comes close to collision when the robot approaches the wheel: the constraints are saturated to prevent it. The constraints are then deactivated when the robot goes away from the obstacle. When the robot starts to rotate the wheel, the constraints become once more active. Since the motion is not holonomic [START_REF] Zanchettin | A general user-oriented framework for holonomic redundancy resolution in robotic manipulators using task augmentation[END_REF] (in particular, there is no posture task), the motion realized for each loop is different: during the second loop, the elbow constraint remains saturated.

Second movement

The robot then uses the second arm to ease the manipulation of the wheel. The motion is then more constrained since both hands are bound to the wheel. Snapshots of the motion are given in Fig. 19. The task sequence is given in Fig. 20: each hand first reaches an arbitrary pre-grasp position before grasping the wheel. During the approach, the three rotations are controlled, while the rotation along the tangential axis to the wheel is left free during the manipulation. The distance to the obstacle is plotted in Fig. 21. The constraint becomes active at the end of the motion. The joint position with respect to the limit is shown in Fig. 22. Contrary to the previous experiment, the joints do not systematically remain on the exact limits since the robot is moving to follow the rotation of the wheel. The number of active constraints is given in Fig. 23. The number of active constraints increases with the complexity of the task. It reaches its maximum just before the robot starts to move the wheel (see Fig. 19-left, the robot is bent on the right with many apparent saturations). It then decreases when the robot stands straight (see Fig. 19middle), and increases again at the end of the motion (see Fig. 19-right). Finally, the conditioning number of the left-and right-hand tasks is shown in Fig. 24.

The conditioning number evolves both continuously with the changes in the Jacobians and discreetly at each constraint activation.

Computation times

Finally, the computation cost of the overall motion is studied. The computation time and corresponding number of iterations of the active set are plotted in Fig. 25. In 97.5% of the cases, the active search loop converges without any update. The mean number of iterations is 0.035 and the maximum is 9. The mean of computation time is 0.9ms. The peaks of number of iteration correspond to peaks of computation time. When 9 iterations are needed, the algorithm takes 3.7ms to converges. This effect emphasizes the fact that the active search is not real time: it is not possible to predict how many iterations are needed to reach the optimum and, even if the mean is below 1ms, a control frequency of 1kHz is not possible. As proposed in Sec. 4.4, it is possible to arbitrarily limit the number of iterations to enforce a deterministic convergence time. In Fig. 26, the algorithm can do only one update at each control cycle. When a second update is requested, the algorithm quits without even For the sake of clarity, the environment is not displayed. computing the dual optimum. The peaks of computation disappear. In exchange, the number of control cycles without update decreases to 96.6%. There is no perceptible changes in the robot behavior, since, due to the continuity properties, the obtained suboptimum is very close to the solution.

Since the only inequalities are at the first levels of the hierarchy and are always compatible, the solver proposed in [De Lasa et al., 2010] can be used to generate the motion. The computation time and corresponding number of iterations of the active-set loops are plotted in Fig. 27. As already noticed with the cascade of QP, the active search needs many iterations to find the optimal active set (up to 30, with a mean at 4.5). However, since each QP solved in the cascade is very small, the number of iterations only slightly impacts the computation cost. In most of the cases, the convergence is slower than in Fig. 25 (the mean is 1.3ms), but there is no peak of computation like with our solver (the maximum is 2.6ms). The computation time is always higher than in Fig. 26.

Experiment C: grasping with posture constraints

This experiment is executed by the real HRP-2 robot.

The robot has to grasp a point object while looking at it and avoiding its joint limits and the collisions with the environment. Three tasks are set at the leastpriority levels to enforce the use of the upper limbs of the robot: the task e legs is regulating the joint positions of the legs to the reference initial position; the task e chest is regulating the orientation of the chest to keep it vertical, using (42). Finally, the last task e up is blocking the upper part of the robot (chest, arms and neck). This kind of behavior using only the necessary limbs to perform an action was proposed in [START_REF] Ee | Whole-body motion generation integrating operator's intention and robot's autonomy in controlling humanoid robots[END_REF] The ball is moved in three different positions, as shown in Fig. 28: first close in front of the robot, then at the height of the waist and finally on the ground behind a small box. The grasping task is finally removed when the last position is reached. The task sequence is shown in Fig. 29. When the ball is close enough, only the least-priority task e up is violated, and the robot is grasping the ball using only its right arm. The neck and the left arm are marginally used, respectively for the FOV task and support task. When the ball is placed at the second position, it is out of the reach of the arm alone. The task e legs is violated at the end of the grasping motion. When the ball is on the ground, it is not possible to grasp it with the chest being vertical. The task e chest is violated at the end of the motion to reach the ball. Finally, the task e rh is removed. The three tasks e legs , e chest and e up are then feasible, and the robot goes back naturally to its initial position.

The errors of the four tasks are given in Fig. 30 and illustrate very well the hierarchical order: the task e rh has priority over the three other ones, and is always accomplished: the error exponentially converges as imposed. The task e up is violated first, and its error is the most important. The task e legs is violated then, while the task e chest is violated at the end, and keeps the lowest error value.

The activation of the limits is synthesized on Fig. 29. Some examples of activations are given in Figures 31 and32. The left hand is moving backward to ensure the robot balance, and is quickly blocked by the task preventing the collision with the wall situated behind the robot. The right hand avoids a collision with the box when going to the third target position. Both collision constraints are deactivated when the robot moves back to the initial pose. Some joints limits of the legs are saturated when the robot goes for the second target position, and many limits are saturated when reaching the third target.

Finally, the number of active inequality constraints, and the number of iterations of the activesearch loop are given in Fig. 33. As previously, the active set of the previous control cycle is used as warm start and reduces the number of iterations of the loop. The maximum number of iterations is 7 and is reached when the task e rh is removed from the hierarchy. In average, 0.035 iterations and 1.1ms are needed at each control cycle, and 97.8% of the cycles are resolved without extra iteration.

Conclusion

In this paper, we have proposed a generic solution to resolve a hierarchical least-square quadratic program defined by equality or inequality constraints. When only equalities are set, the method comes back to the classical stat-of-the-art solutions [Hanafusa et al., 1981, Siciliano and Slotine, 1991, Baerlocher and Boulic, 2004] but is up to ten times faster. When the problem encompasses inequality constraints, a true hierarchy is solved with inequalities at any level. The resolution loop keeps a low number of iterations by using a unified active-search algorithm, in contrast to the cascades of QP used in [Kanoun et al., 2011, De Lasa et al., 2010]. Using a proper construction of the active-search loop, the resolution can be performed in real-time at 200Hz using a classical personal computer and no specific hardware tuning.

The solver is generic and can be applied to many problems in robotics and beyond. We have proposed to apply it to compute the control law of a redundant robot in the inverse-kinematics context. In that case, we have shown that the control is continuous for a given hierarchy, and stable. The method was used to generate several complex motions with the humanoid robot HRP-2, in presence of realistic constraints such as joint limits, obstacle or field of view. For most of the control cycles (97% in average in the presented experiments), the active-search loop does not need any iterations, which implies that the solver deals with inequalities as if they were equalities. The presented experiments validated that the proposed method is the first complete solution to handle inequalities at any level inside a hierarchy of tasks to generate robot motions in real time.

In the future, we will try to reduce the computation cost by predicting the future constraint activations. The solver was only applied to inverse the robot kinematics, but the dynamics could be handled as well. Only a simple obstacle-avoidance scheme was set up, and a proper link with a complete collision checker should be studied. Finally, the continuity of the control scheme is not ensured when adding or removing a task from the hierarchy, which is needed before being able to apply it as a basic solution on our robots. Openings to other kind of problems, such as those solved by a walking pattern generator, is also an important perspective.

A Index to Multimedia Extensions

B Elementary transformation

This appendix recalls the basis on Householder reflections and Givens rotations.

B.1 Householder reflections

Householder reflections are orthogonal transformations built from any vector v:

Q = I - 2 v T v vv T ( 46
)
If v is chosen as v = (0, . . . , 0, a 1a , a 2 , . . . , a n ), then:

Q             × . . . × a             =             × . . . × a 0 . . . 0             (47)
Householder reflections are rank-1 modifications of the identity. They are very useful to zero the tail of a vector or of a matrix column. They do not affect the head of the vector they are applied on. The explicit shape of Q should not be computed, but only v should be stored. See [Golub and Van Loan, 1996] for more details.

B.2 Givens rotations

Givens rotations of any dimension d are defined by: when θ = tan -1 (x j , x i ). Givens rotations are rank-2 modifications of the identity.

G[i,
See [Golub and Van Loan, 1996] for more details. In this paper, Householder and Givens matrices are referred to as elementary transformations.

C Proof of Theorem 4.3

We define the Lyapunov energy function by V = 1 2 e E T e E . Then V = e E T ėE = e E T J E q (50)

where q is computed from the eHQP of the current optimal active search:

q = Y H ‡ p W T p b p (51)
Suppose that the active set of S is built incrementally from the initial guess S E , using the update procedure of Sec. 3.2. The initial decomposition involves only J E , with the following notations:

J E = W E H E Y T E (52)
For each active component of the inequality levels S I , there are two options: it corresponds to a column Y i of Y E (case 2.2 of the update) or it is decoupled. In the second case, the component can be equivalently activated at the least-priority level. The optimum is then equivalently written: where the τ k are the step lengths met during the update phases and ∆ τ is the diagonal matrix of the 1τ k .

y * = (
Introducing this last form into (50):

V = e E T W E H E ∆ τ y * E (55) = e E T W E H E ∆ τ H ‡ E W T E e E ( 56
)
which is non-negative since ∆ τ is positive and H ‡ E respects the three first conditions of Moore-Penrose.

If J E is full rank, all the N of H k are empty. Moreover, if none of the equality levels of the iHQP are rank deficient, then it is as if all the inequalities had been activated at a least-priority level p. Then:

V = e E
T e E > 0 (57) which ensures the asymptotic stability.

D MATLAB code

The main algorithms proposed in the paper are summarized by a short Matlab solver attached to it. The software solves the following HQP: lex min

x,w1...wp { w 1 , w 2 , . . . , w p } . • eHQP primal computes the primal optimum of the eHQP corresponding to the current active set. The function returns the optimum x * and the corresponding expression in the Y basis y * . This corresponds to Alg. 1

• eHQP dual computes the multiplier λ k of one level k of the hierarchy, as proposed in Alg. 2.

• active search is the main function: it computes the HCOD and then runs the search loop. It returns the primal and dual optima and the HCOD. It uses the fuctions step length (see Alg. 4), check multipliers and freeze (to test the lexicographic positivity of the multipliers), and up and down (to add and remove one row of the HCOD).

To simplify the code, the solver computes explicitly the left and right bases of the HCOD instead of using structured matrices. It is given as a detailed example to guide the implementation of a hierarchical QP solver, but should not be used for reactivity tests. An optimized version of the solver coded in C++ using the Eigen library and structured-matrix bases was used for the experiments.

Algorithm 2

 2 Dual eHQP of level k 1: function eHQP dual(A p , b p , x * , k) 2: Input: HCOD of A p , b p , Primal optimum x * , level k 3: Output: w * k and λ k satisfying (32) and (35) 4: e = N k y * k -V k b k 5: w * k = V k e 6: ρ = -N T k e 7: for j=k-1 downto 1 do

Fig. 2 :

 2 Fig.2: An example of execution in two dimensions. The initial point is x(0) . The iterations of the algorithm are denoted by x (1) ...x(7) . The optimum is finally reached in x(7) . The constraints of the first levels are always respected, and the path is slidding along these constraints. From x (3) , all the constraints of level 2 are satisfied or active. The first deactivation then occurs. The details of the execution are given in App. B.

Algorithm 5

 5 Hierarchical active search 1: Input: Initial guess S (0) 2: Output: x * minimizing (27) 3: x = 0 ; S = S (0) 4: F = ∅ 5: for k = 1 : p do

F

  necessary, decrease the active set--18: w, λ = eHQP dual (A p , b p , k, S) := S {cst, λ cst > 0} {cst, w cst > 0}

Fig. 3 :

 3 Fig. 3: Walking with an umbrella: snapshots from a top view. (a) the robot manages to keep its companion below the umbrella (b) the robot does not manage to accomplish the secondary task while following the references of the walking pattern generator.

Fig. 5 :

 5 Fig. 5: Walking with an umbrella: normalized positions of some typical joints. The joint limits are respected at any time.

Fig. 1 :

 1 Fig. 1: Various situations of inequality and equality constraints. (a) Obstacle avoidance, joint limits and support polygon. (b) Reaching an object behind an obstacle while ensuring the robot balance. The visibility is only ensured if possible.

  up = I and stops with Y up = Y (r-r k ) up

Fig. 2 :

 2 Fig.2: Continuity and stability: when the system approaches a boundary, the optimum continuously changes to a pure motion along the boundary, until it finally stably stops on the limit. The optimum does not fight against the reference required by the equality constraint of level 2.

Fig. 3 :

 3 Fig. 3: Comparison of the computation time of the eHQP primal optimum using four methods. (a) Using MP with a SVD (b) Using MP with a COD (c) Using MZ with a COD (d) Using MH. Various problems A with n = 100, m = mi = 120, r = ri = 80 are resolved using varying number of levels p on a 2.5GHz PC processor. The computation time is plotted wrt p. The plots display the time measured for the decomposition of AP and AZ only (D), for the decomposition and the inversion (D+I), and for decomposition, inversion and projection (total cost). In (a) and (b) (D) is nearly equal to (D+I).In (d), the projection consists in translating y * in the canonical basis, which is plotted but is negligible.

Fig. 4 :

 4 Fig.4: Computation cost of the iHQP resolution using a cascade of QP[Kanoun et al., 2011] or a our HQP solver (Alg. 5). A set of hierarchical problems is solved, whose size parameters are the same (n = 100, m = 150, r = 80) but whose number p of levels varies. The computation time is plotted wrt p. The time increases with the number of levels when using a cascade, while it is nearly constant with our solver. A little overhead appears for high p and is due to the computation of Λp. For p = 1, the cost are exactly the same.

Fig. 5 :

 5 Fig.5: Number of activations and deactivations using a cascade of QP and a our solver for the same problems as in Fig.4. The number increases with p for the cascade of QP and remains constant for the HQP solver.

Fig. 6 :Fig. 7 :Fig. 8 :Fig. 9 :

 6789 Fig. 6: Top row: snapshots of the robot motion. Bottom row: corresponding COM projection (blue point) in the support polygon (brown rectangles depict the feet, the blue rectangle shows the area for e bal ). Each snapshot is captured at the end of a motion sequence. The FOV is displayed by the 4 lines passing by the center of the image projection.

Fig. 10 :Fig. 11 :Fig. 12 :

 101112 Fig. 10: Experiment A: normalized joint positions.

Fig. 13 :Fig. 14 :

 1314 Fig. 13: Experiment A: Number of algorithm iterations and computation time when no initial guess is used.

  when a new boundary is reached. The maximal number of iterations is 6 (at the first iteration after the change of the ball position at T=7), the mean number is 0.03 and in 97.6% of the case, the active search converges without any update. As shown in Sec. 4.1, the computation time depends on the number of active constraints and of active-search iterations. Since there is nearly no iteration, Fig.14-(bottom) depends only on Fig. 11 and has the same shape. In Figures 12-(bottom) and 13-(bottom), the influence of the number of iterations is more important, and the time graph shape is similar to the graph of number of iterations. Using the HQP and the warm start, an average of 0.56ms of computation is needed.

Fig. 16 :

 16 Fig. 15: Experiment B-1: Snapshots of the first movement: the robot uses only its left hand to manipulate the wheel. The three snapshots are captured during the wheel rotation for three angles of 0, 2π 3 and 4π 3 .

Fig. 17 :

 17 Fig. 17: Experiment B-1: Number of active inequalities during the first movement.

Fig. 18 :

 18 Fig.18: Experiment B-1: distance to the obstacle during the first movement. The obstacle is on the way between the left-arm initial position and the wheel: the two constraints of the shoulder and the elbow become active at T = 1.1s and T = 1.2s. They are deactivated later during the grasping phase, at T = 2s and T = 2.1s. At each loop of the wheel, the left arm comes close to the obstacle, between T = 4s and T = 5s, and a second time between T = 5.7s and T = 7s.

Fig. 19 :

 19 Fig.19: Experiment B-2: Snapshots of the second movement: the robot uses both hands to manipulate the wheel. For the sake of clarity, the environment is not displayed.

Fig. 20 :

 20 Fig.20: Experiment B-2: Task sequence of the second movement: the plots show the grasping phase (from T = 9s to T = 13.5s) and a rotation of 2π 3 . For the four inequality tasks, the number of active constraints during the motion is plotted for each levels.

  Fig. 21: Experiment B-2: Distance to the obstacle.

Fig. 22 :

 22 Fig. 22: Experiment B-2: Normalized joint positions during the second movement.

Fig. 23 :

 23 Fig. 23: Experiment B-2: Number of active inequalities during the second movement.

Fig. 24 :Fig. 25 :

 2425 Fig. 24: Experiment B-2: Conditioning number of the hand tasks when both hands are moving the wheel. The vertical dot lines show the inequality activations. The conditioning number evolves both continuously with the changes in the Jacobians and discreetly at each constraint activation.

Fig. 26 :Fig. 27 :

 2627 Fig. 26: Experiment B: Number of algorithm iterations and computation time when using a warm start and a limitation of active-set iterations.

  using dedicated geometrical computations. The hierarchy is e jl ≺ e coll ≺ e supp ≺ e rh ≺ e f ov ≺ e legs ≺ e chest ≺ e up . The motion is summarized by Figures 28 to 33

  .

Fig. 28 :Fig. 30 :

 2830 Fig. 28: Experiment C: Snapshots of the robot movement.

  Fig. 31: Experiment C: Distance to the obstacle.

Fig. 33 :

 33 Fig. 33: Experiment C: Number of active inequalities and updates.

  j, θ] = I d -S ij (I 2 -R θ )S T ij (48) with I d and I 2 the identity matrices of dimension d and 2 respectively, R θ = cos θ sin θ -sin θ cos θ the 2Drotation of angle θ and S ij = e i e j with e i and e j the canonical vectors (zero everywhere except on the corresponding row). A right multiplication with a Givens rotation only modify the two rows i and j.It can be used to introduce a zero inside a vector or a matrix:

  y * 1 , . . . , y * p , y * p+1 )(53)with y * p+1 the equivalent level where all the decoupled constraints of S are set and all the active inequalities of levels 1 to p are coupled with J E : both bases Y and Y E are the same with a permutation Π on the column order:Y = Y E Π.The optimum y * p of the full iHQP can be computed from the optimum of the eHQP corresponding to J E , denoted by y * E :

  each constraint, b is given by a pair of floats and an integer identifying the constraint to one of the four constraint types specified above. The problem specifications A p , b p , the current active set and the corresponding HCOD are stored in a dedicated array h: each cell of the array corresponds to one level of the hierarchy and stores A k , b k , W k , H k and the active constraints. The right basis Y is independently stored. The software is organized in four main functions split in the same number of files: • hcod computes the HCOD from the problem specification A p , b p and an initial guess of the active set. The function returns h and Y that are manipulated by the other function during the active-search loop.

Table 1 :

 1 Computation cost for each operation of the active search. The approximations are given for n = m and k = p

The principle of the active search algorithm is recalled in App. A.

For clarity purpose and without loss of generality, the problem is formulated with upper-bound constraints only. Efficient implementations would make explicit lower bounds, double bounds and equality (twin bounds) constraints. See App. D for details.

All the algorithms being given in the first part of the paper, they will be simply referenced by their number.

Acknowledgments

This work was supported by grants from the RobotHow.cog EU CEC project, Contract No. 288533 under the 7th Research program (www.robohow.eu), and French FUI Project ROMEO. Many thanks to C. Benazeth, A. El Khoury, F. Keith, A. Kheddar and F. Lamiraux for their help. Acknowledgments This work was supported by grants from the RobotHow.cog EU project, Contract No. 288533 under the 7 th research program (www.robohow.eu), and French FUI Project ROMEO. Many thanks to C. Benazeth, A. El Khoury, F. Keith, A. Kheddar and F. Lamiraux for their help.