Stanislas Brossette

Adrien Escande

Senior Member, IEEE Abderrahmane Kheddar

Multi-Contact Postures Computation on Manifolds

Keywords: Robotics, Posture Computation, Contacts, Optimization on Manifolds

We propose a framework to generate static robot configurations satisfying a set of physical and geometrical constraints. This is done by formulating nonlinear constrained optimization problems over non-Euclidean manifolds and solving them. To do so, we present a new sequential quadratic programming (SQP) solver working natively on general manifolds, and propose an interface to easily formulate the problems, with the tedious and error-prone work automated for the user. We also introduce several new types of constraints for having more complex contacts or working on forces/torques. Our approach allows an elegant mathematical description of the constraints and we exemplify it through formulation and computation examples in complex scenarios with humanoid robots.

I. INTRODUCTION

C OMPUTING static robot configuration that fulfills a set of objectives is needed in many aspects of robotics. This is crucial in humanoid robotics, where it has been used to find feasible multi-contact postures in planning e.g. [START_REF] Escande | Planning support contactpoints for humanoid robots and experiments on HRP-2[END_REF], [START_REF] Hauser | Motion planning for legged and humanoid robots[END_REF], [START_REF] Escande | Planning contact points for humanoid robots[END_REF], to provide low-priority reference postures in control [START_REF] Vaillant | Multi-contact vertical ladder climbing with an HRP-2 humanoid[END_REF], to explore feasible head position for object reconstruction [START_REF] Stasse | Towards autonomous object reconstruction for visual search by the humanoid robot HRP-2[END_REF] or to discover the surroundings [START_REF] Foissotte | Autonomous 3d object modeling by a humanoid using an optimizationdriven next-best-view formulation[END_REF].

For a given robotic system, we consider the problem of finding a single configuration to achieve a set of objectives T i . We denote q the joint configuration (n joints plus a reference base) of the robot, ξ = ξ T 1 , . . . , ξ T m T represents the m contact forces applied on the robot, and y denotes additional variables. Each objective T i can be represented by a set of equality and inequality equations:

g i (q, ξ, y) = 0 h i (q, ξ, y) ≥ 0 (1)
The extra variable y is useful to express many constraints [START_REF] Brossette | Integration of non-inclusive contacts in posture generation[END_REF] as explained later (section V). In addition to satisfying the tasks T i , the configuration we seek for -we call it posture- must represent a feasible state of the robotic system. This translates into the following feasibility problem:

Q :                                q - i ≤ q i ≤ q + i ∀i ∈ [1, n] τ - i ≤ τ i (q, ξ) ≤ τ + i ∀i ∈ [1, n]
ij ≤ d(r i (q), r j (q)), ∀(i, j) ∈ I auto ik ≤ d(r i (q), O k ), ∀(i, k) ∈ I coll s(q, ξ) = 0 c(ξ i ) ≥ 0 i ∈ [1, m] g i (q, ξ, y) = 0

h i (q, ξ, y) ≥ 0 (2)
(3)

(5)

(8) [START_REF] Baerlocher | Parametrization and range of motion of the ball-and-socket joint[END_REF] where equations ( 2) and ( 3) are the joint and torque limits respectively; eq. ( 4) disallows auto-collision between a pair of robot bodies {r i , r j } given by a set I auto and where d is the distance function; eq. ( 5) forbids collisions between a robot body r i and an object of the environment O k defined in a set I coll ; ij and ik are the acceptable distance thresholds. Eq. ( 6) ensures the stability of the robot, by requiring the balance of external forces and torques applied to the robot. Finally, the contact forces must lie within the Coulomb friction cones, which is enforced by eq. [START_REF] Brossette | Integration of non-inclusive contacts in posture generation[END_REF]. We illustrate these constraints in Fig. 1. The set Q, if not empty, usually contains infinitely many feasible postures and we may want to pick-up one according to 0000-0000/00$00.00 c 201X IEEE a cost function f . This yield a nonlinear optimization problem that we name Posture Generation (PG) problem: min.

q,ξ,y f (q, ξ, y) s.t. (q, ξ, y) ∈ Q [START_REF] Luenberger | The gradient projection method along geodesics[END_REF] Usually, the PG problem is formulated over a Euclidean space. However, robot variables may be more appropriately expressed in non-Euclidean manifolds. An example of a typical manifold is the rotation space SO [START_REF] Escande | Planning contact points for humanoid robots[END_REF], which is used to represent the orientation of the robot root, or to parametrize a ball joint. Another example is the 3D unit sphere on S 2 which is used to express the contact with a parametric surface in [START_REF] Escande | Parametrization of catmullclark subdivision surfaces for posture generation[END_REF]; in [START_REF] Baerlocher | Parametrization and range of motion of the ball-and-socket joint[END_REF] a human shoulder is parametrized on S 2 × R.

Formulating a non-Euclidean manifold optimization problem over R n leads either to singularities or to the addition of constraints and variables to this problem, as detailed in the next section. Since manifolds are locally Euclidean, the properties of distance, derivatives, and more generally all the Euclidean geometry can be used locally. Usual unconstrained optimization methods have been adapted to work on non-Euclidean manifolds (e.g. [START_REF] Luenberger | The gradient projection method along geodesics[END_REF], [START_REF] Stuart | Dynamical systems and numerical analysis[END_REF], [START_REF] Gabay | Minimizing a differentiable function over a differential manifold[END_REF], [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF]). However, posture generation requires using constrained optimization together with manifolds. To the best of our knowledge, there is no method to solve such class of problems with generic manifolds (for specific ones, [START_REF] Schulman | Motion planning with sequential convex optimization and convex collision checking[END_REF] proposes an extension of a classical approach to the special case SE(3)).

Our primary contribution consists in a Sequential Quadratic Programming (SQP) solver working directly on manifolds. We were largely inspired by the seminal work of Absil [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF] for unconstrained optimization, that we adapt to the constrained case by borrowing from the existing literature, and in particular from the work of Fletcher [START_REF] Fletcher | Nonlinear programming without a penalty function[END_REF]. A background motivation is to have our own optimization solver, that we can customize at will for robotic problems, leveraging modeling properties and approximations for a gain in time and robustness. The theoretical development and the sketch of our SQP on manifolds are given in Sec. II, while the different refinements needed to have a fully working solver are explained in Sec. III.

The definition of a PG problem as an optimization problem is often a cumbersome task. Our second contribution is a framework simplifying and automating part of this task so that the user can focus on the mathematical formulation of the constraints (Sec. IV). It makes it as convenient as possible to write custom PG problems and to develop custom constraints and cost functions. We take advantage of it to introduce new constraints for having contact on parametric surfaces and formulating force-based tasks (Sec. V). We also provide an efficient algorithm to compute the derivative of the torques with respect to the problem's variables. Finally, we present some evaluations of both the solver alone and the posture generator, as well as some scenarios illustrating their combined capabilities (Sec. VI).

Part of this paper was presented in [START_REF] Brossette | Humanoid posture generation on non-Euclidean manifolds[END_REF]. The material of secs. II and IV are extended version of the previous work with more details and a few corrections. The rest of the paper is almost completely new. The example of Fig. 10 is borrowed from [START_REF] Brossette | Humanoid posture generation on non-Euclidean manifolds[END_REF], the one from Fig. 11 is reworked from [START_REF] Escande | Parametrization of catmullclark subdivision surfaces for posture generation[END_REF], with the addition of collision avoidance.

II. CONSTRAINED OPTIMIZATION ON MANIFOLDS

We consider the generic problem min.

x∈M f (x) (11) subject to l ≤ c(x) ≤ u
where M is a n-dimensional smooth manifold and f and c are 1and m-dimensional real-valued C 1 functions. Equality constraints are formulated by taking l i = u i . Except for specific cases, the numerical approach to solve problem [START_REF] Stuart | Dynamical systems and numerical analysis[END_REF] for M = R n is iterative: it starts from an initial guess x 0 and performs successive steps x (k+1) = x (k) + p (k) , until convergence. The strategy to compute p (k) at each iteration depends on the solver. This scheme cannot be readily applied to optimization over non-Euclidean manifolds. In what follows we explain why, and give an adaptation of the SQP approach to tackle general manifolds.

A. Representation problem

To manipulate elements of M we need a way to represent them in memory. We choose a representation space E = R r (with r ≥ n) and a map, that is ψ :

x

→ x M → E
According to the Whitney embedding theorem [START_REF] Lee | Introduction to smooth manifolds[END_REF], one can take ψ as a diffeomorphism onto ψ(M) provided that r is large enough. In the following, we identify abusively M with ψ(M) ⊆ E. Once the choice is made, we can also define a projection π from E to ψ(M).

With this representation, it is tempting to simply transform the problem [START_REF] Stuart | Dynamical systems and numerical analysis[END_REF] as an optimization over R r with the objective f • ψ -1 and the constraint c • ψ -1 , to be resolved with a usual solver. But depending on the representation choice, one of the two following problems arises: (i) r = n, then it is not possible in the general non-Euclidean case to find ψ without derivative discontinuities, which can lead to critical convergence problems; (ii) r > n, then most elements of E do not represent an element of M and ψ cannot be surjective. To force the solution on M, we need to add constraints equivalent to π(x) = x. As a result, the problem has more variables and constraints w.r.t (i). Moreover, the additional constraints are unlikely to be met along the iteration process (even if x (k) is an element of M, x (k) + p (k) is likely not, as nothing enforces it). This means that in order to evaluate f • ψ -1 and c • ψ -1 at a given x (k) , one has to project it on ψ(M) first, effectively computing

f • ψ -1 • π and c • ψ -1 • π.
The composition by π is an additional burden in programming (see e.g. in [START_REF] Bouyarmane | On the dynamics modeling of free-floating-base articulated mechanisms and applications to humanoid whole-body dynamics and control[END_REF]).

As an example, the set of 3D-rotations SO(3) is a manifold of dimension 3. The following usual choices can be made:

• Rotation matrix R ∈ R 3×3 ≈ R 9 , additional constraints: {R T R = I , det(R) = 1}, projection by orthogonaliza- tion, • Quaternion q ∈ R 4 , additional constraints: { q = 1}, projection π(x) = x/ x , • Euler angles (E = R 3
), singularities when reaching gimbal lock.

• Spatial twist or angle-axis (E = R 3 ), singularities when the rotation is 2kπ, k = 0. For the last two cases, the convergence in the vicinity of a singularity can be disturbed because two infinitesimally close rotations can have very different representations (see example in Appendix A).

B. Local parametrization

Given x ∈ M, we note T x M the tangent space of M at x. It is a linear space, identifiable with R n . By definition of a smooth manifold, there exists a map ϕ x from an open set U of T x M to an open set of M such that ϕ x (0) = x. ϕ x provides a local parametrization for M. Should the solution of (11) lies in ϕ x (U ) for a given x, we could transform the objective and constraints into f •ϕ x and l ≤ c•ϕ x (z) ≤ b and simply solve a problem over R n with an off-the-shelf solver. However, finding a point x close enough to the solution is a problem in itself. A more systematic approach, and the driving idea of optimization on manifold, is to change the parametrization at each iteration: for each iterate x (k) , we reformulate the problem as min.

z∈T x (k) M f • ϕ x (k) (z) (12) subject to l ≤ c • ϕ x (k) (z) ≤ b which is a classical optimization problem over R n . Starting from z = 0 (recall that ϕ x (k) (0) = x (k) )
, we can perform one iteration to get z (1) , set x k+1 = ϕ x (k) (z (1) ) and reparametrize. The process is repeated until convergence is achieved. In the computation of the step z (1) , one needs to be careful about the local validity of the map: if ϕ x (k) is defined on U k ⊂ T x (k) M, then z (1) must be restricted to U k .

Fig. 2. There are many possible choices for ϕx but not all yield to a curve ϕx(tz) which is going in the same direction as z: ϕ 1 and ϕ 2 are correct choices, ϕ 3 is not.

At any point x (k) , there is an infinite number of possible maps ϕ x (k) (see Fig. 2), most of which are unsuitable to the optimization process. A suitable map needs to translate the direction and magnitude of z onto M: the tangent vector at t = 0 of the curve t → ϕ xi (tz) must be z. This is known as the local rigidity condition. This led to the concept of retraction [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF], a smooth mapping ϕ : (x, z) → ϕ x (z) such that for every x 1)

ϕ x (0) = x 2) ∀z ∈ T x M, dϕ x (tz) dt t=0 = z
Even when restricting to retractions, there are still many choices of (family of) maps. The exponential map is an obvious candidate, but in the general case it is very expensive to compute, because it amounts to solving a differential equation. The literature offers cheaper alternatives for many classical manifolds (see e.g. [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF], [START_REF] Boumal | Manopt, a matlab toolbox for optimization on manifolds[END_REF]). Using a retraction ϕ, the iterative process uses the minimum number of variables and avoids additional constraints and parametrization-related singularities. Yet, we need to keep track of the iterates x (k) globally. Since the x (k) are guaranteed to be on M, we can use a space E = R r and a map ψ, with r > n, without any drawback. The user can write the functions f = f • ψ -1 and c = c • ψ -1 as if they were functions from E without using a projection π beforehand. When using a new manifold, one needs to program once and for all ψ • ϕ. The Fig. 3 sums-up the different spaces and mappings. Fig. 3. Given a manifold M, we use a global map ψ from an embedding space E to track its elements. π projects a generic element of E on ψ(M). Given x ∈ M, we also define a local map ϕx with which we can perform the computations. The functions actually implemented are f and ψ • ϕ, which are functions between Euclidean spaces.

C. A basic SQP on manifold

There are several ways to compute a step z (1) . We adopt the SQP approach which consists in making a quadratic-linear approximation of the problem [START_REF] Gabay | Minimizing a differentiable function over a differential manifold[END_REF] around the current iterate, corresponding to z = 0, and solving it.

Let us first define the Lagrangian function

L x = f • ϕ x -λ T -(c • ϕ x -l) + λ T + (c • ϕ x -u) (13)
where λ -, λ + ∈ R m are the Lagrange multipliers associated with the lower and upper bounds respectively 1 . Let H (k) be the Hessian matrix ∇ 2 zz L x k . Then the approximation writes as a Quadratic Program (QP) min.

z∈R n ∂f • ϕ x (k) ∂z (0) T z + 1 2 z T H (k) z (14) subject to l ≤ c • ϕ x (k) (0) + ∂c•ϕ x (k) ∂z (0)z ≤ u
The basic SQP approach adapted to manifolds is as follows 1) set k = 0, and set x (k) to the initial value 2) solve the QP [START_REF] Schulman | Motion planning with sequential convex optimization and convex collision checking[END_REF] for current x (k) to get z as well as the QP's Lagrange multipliers λ QP -and

λ QP + 3) set x (k+1) = ϕ x (k) (z), λ (k+1)
- = λ QP -, λ (k+1)
+ = λ QP + and k = k + 1 4) if the convergence is not yet achieved go to step 2
Computations of function's values and derivatives are based on the fact that f • ϕ x = f • ψ • ϕ x (and same for c), and

f : E → R ψ • ϕ x : R n → E are functions between Euclidean spaces. The gradient of f • ϕ x is ∂f • ϕ x ∂z = ∂f ∂y (ψ • ϕ x ) × ∂(ψ • ϕ x ) ∂z (15)
Because the problem is reparametrized at each iteration, the derivative of ψ • ϕ x is always computed at z = 0, which usually allows for some fast specialized code.

The approach taken here is in theory applicable to any smooth manifold (including products of such manifolds), and is usable in practice whenever tractable retractions ϕ and vector transports (see below) can be implemented.

III. PRACTICAL IMPLEMENTATION

The algorithm given in Sec. II-C is conceptual and works correctly when the initial iterate is close enough to the solution. In practice, several refinements need to be implemented to avoid divergence, deal with possible infeasibility in the QP approximation, alleviate the computations and speed-up the process. In this section, we detail these refinements. For each of these issues, several solutions exist, sometimes interdependently. We mostly follow the approach of Fletcher [START_REF] Fletcher | Nonlinear programming without a penalty function[END_REF], that we adapt to work with manifolds, with some choices motivated by experiments on robotics problems, such as those described in Section VI. We provide a working and coherent set of refinements, carefully chosen and adapted to the context of non-Euclidean optimization from the wealth of the literature.

A. Convergence criterion

The convergence is considered to be achieved when the KKT conditions are satisfied 2 (see [START_REF] Nocedal | Numerical Optimization[END_REF]). With the sign convention taken in definition [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF], the KKT conditions for [START_REF] Stuart | Dynamical systems and numerical analysis[END_REF] are (with quantities taken at z = 0):

∇ z L x = ∇ z f • ϕ x + (∇ z c • ϕ x ) T (λ + -λ -) = 0 l ≤ c • ϕ x ≤ u λ -≥ 0, λ T -(c • ϕ x -l) = 0 λ + ≥ 0, λ T + (c • ϕ x -u) = 0
In practice, we account for floating point computations using tolerances that we scale to the problem. Given two user defined tolerance parameters τ P and τ D , and following [START_REF] Gill | Snopt: An sqp algorithm for large-scale constrained optimization[END_REF], we define

τ x = τ P (1 + x ∞ ) and τ λ = τ D (1 + λ ∞ )
, where λ = λ + -λ -. Our stopping criterion is

∇ z L x ≤ τ λ (16) ∀i    |c i (x) -l i | ≤ τ x and λ i ≤ -τ λ or l i + τ x ≤ c i (x) ≤ u i -τ x and |λ i | ≤ τ λ or |c i (x) -u i | ≤ τ x and λ i ≥ τ λ (17)
2 These conditions only ensure that we reached a critical point, not necessarily a minimum, much less the global minimum.

B. Globalization

The approximation ( 14) might be accurate only in a small neighborhood of x (k) , so that the step found is not correct. The basic SQP algorithm might not converge, or even diverge rapidly. Adapting the algorithm to enforce its convergence (at least theoretically and not necessarily to the solution) is called globalization. It consists of two components: (i) a criterion to assess the correctness of the step, and (ii) a method to modify it if needed. In constrained optimization, the most classical criterion is to take a penalty function Φ(x, p) aggregating the objective and the violation of the constraints, where p is a penalty parameter balancing between objective and violation. A step is correct if Φ(x (k+1) , p) < Φ(x (k) , p). For the method to converge to the solution, the parameter p needs to be adapted along the iterations and here lies the difficulty of this approach. In our early tests, we had troubles obtaining convergence on robotics problems. Instead, we opted for a quite recent criterion (see [START_REF] Fletcher | Nonlinear programming without a penalty function[END_REF]) based on filters.

Let

v i (x (k) ), i = 1 • • • m be defined as l i -c i (x (k) ) if c i (x (k) ) < l i , c i (x (k) ) -u i if c i (x (k) ) > u i and 0 otherwise. It represents the violation of the i-th constraint. Noting v = v i
, the optimization process can be seen as trying to satisfy two possibly conflicting objectives: decrease f and decrease v. The idea of the filter approach is to accept a step whenever the new iterate does not do worse than any previous one on both objectives, and does marginally better for one objective than at least one previous iterate. More precisely, we say that

x i dominates x j if f (x i ) ≤ f (x j ) -γv(x j ) or v(x i ) ≤ (1 -γ)v(x j )
, where γ is a small number (typically 10 -5 ) and the terms in γ translates the "marginally better" concept. A filter is a set of values x i where no element dominates another. A filter dominates an iterate x if any of its elements dominates x. We start with an empty filter F ; at iteration k the step z is accepted if the new iterate x (k+1) = ϕ x (k) (z) is not dominated by F . In this case, x (k+1) is added to F , and all the elements of F it dominates are removed from the filter. If x (k+1) is dominated by F , the step is rejected and has to be modified.

There are two big classes of modifying methods: line-search and trust-region. In our case, we already have a notion of trust-region as we need the step z to remain within the set U k on which ϕ x (k) is defined, so we chose this method. The trust-region approach consists in restricting z to be in a region where the QP approximation ( 14) is "good enough". To simplify the approach and retain a QP formulation, we consider a rectangular trust region: given a user-defined vector z t ∈ R n with z t ≥ 0, we want -ρz t ≤ z ≤ ρz t , where ρ is a scalar accounting for the confidence we have in the QP approximation. When a step is rejected by the filter, we modify the trust region by taking ρ = max(ρ/2, ρ min ). If a step is accepted, and it was constrained by the trust region (i.e. z i = ±ρz t,i for some i), we take ρ = min(2ρ, ρ max ). ρ min , ρ max and the initial ρ are user-defined. Typical default value are 10 -8 , 2 and 1. z t encodes the typical step size of each variables. It allows to consider variables of different scales, by having an anisotropic trust-region, without having to write the functions f and c to take into account those scale differences.

For example, on a 60kg humanoid robots with joint and force variables, we typically take 0.1 for joints and 10 for forces.

The region U k is also considered as rectangular: z - u ≤ z ≤ z + u . We then defined z -= max(z - u , -ρz t ) and z + = min(z + u , ρz t ), and QP ( 14) with a trust-region is given by: min.

z∈R n ∂f • ϕ x (k) ∂z (0) T z + 1 2 z T H (k) z (18) subject to l ≤ c • ϕ x (k) (0) + ∂c • ϕ x (k) ∂z (0)z ≤ u (19) z -≤ z ≤ z +
C. Restoration and second order correction QP ( 18) can be infeasible because some constraints in ( 19) are incompatible or, more often, because the feasible points for [START_REF] Boumal | Manopt, a matlab toolbox for optimization on manifolds[END_REF] are outside of the trust region. When this is detected, we trigger a restoration phase whose goal is to reduce the violation of the constraints l ≤ c(x) ≤ u until QP [START_REF] Bouyarmane | On the dynamics modeling of free-floating-base articulated mechanisms and applications to humanoid whole-body dynamics and control[END_REF] becomes feasible again. We follow the approach in [START_REF] Fletcher | Nonlinear programming without a penalty function[END_REF].

Infeasibility is detected by the QP solver when attempting to solve QP [START_REF] Bouyarmane | On the dynamics modeling of free-floating-base articulated mechanisms and applications to humanoid whole-body dynamics and control[END_REF]. From the solver output, we can define 2 lists of indexes I l and I u corresponding to constraints whose lower or upper bound cannot be satisfied, and a list F corresponding to feasible constraints. The idea of the restoration phase is to begin to solve the problem min.

x∈M i∈I l (l i -c i (x)) + i∈Iu (c i (x) -u i ) (20)
subject to

l rest ≤ c(x) ≤ u rest (21)
with l rest i = -∞ if i ∈ I l and l i otherwise, and likewise l rest i = +∞ if i ∈ I u and l i otherwise. This corresponds to minimizing the violation of the infeasible constraints while enforcing the feasible ones.

The above problem is nonlinear, and we solve it with the same SQP. The restoration phase has its own filter F rest and its own Hessian approximation (see next subsection), but shares the trust region parameter with the main phase. The difference with the main phase is that we stop to iterate as soon as we detect that QP ( 18) is feasible again. Also, at each iteration the sets I l , I u and F can be updated to account for (linearized) constraints becoming feasible or infeasible in [START_REF] Bouyarmane | On the dynamics modeling of free-floating-base articulated mechanisms and applications to humanoid whole-body dynamics and control[END_REF]. For more details, the interested reader can refer to [START_REF] Fletcher | The sequential quadratic programming method[END_REF].

In restoration, if a step z is rejected by the filter F rest , we attempt first to solve a modified QP problem and reduce the radius of the trust region only if the new step is rejected. In this modified QP, a linear term is added to the constraints [START_REF] Boumal | Manopt, a matlab toolbox for optimization on manifolds[END_REF] which approximates the second order in the Taylor development of c • ϕ x . This is known as the second-order correction (SOC) [START_REF] Fletcher | Nonlinear programming without a penalty function[END_REF]. SOC can also be used in the main phase, but, in our tests, we didn't find a particular advantage to use it.

We exit the restoration with an iterate x (k) , that we add to the main filter F . In case x (k) is dominated by some elements of F , we remove them, what we refer to as forcing the filter.

D. Hessian computation

The computation of the exact Hessian matrices H (k) or H (k) j (for each constraint and the cost function) is costly. It is common to only approximate them based on the previously computed steps and gradients. This is known as the quasi-Newton approach. In usual Euclidean settings, we define s (k) = x (k+1) -x (k) and y (k) = ∇L x (k+1) -∇L x (k) , and, given an approximation Ĥ(k) of H (k) , the approximation of H (k+1) is given by Ĥ(k+1) = Ω( Ĥ(k) , s (k) , y (k) ) where the function Ω is light to compute (it is typically a rank-1 or rank-2 update of Ĥ(k) ) and depends on the method.

With manifolds, both definitions of s (k) and y (k) are usually not valid, the former because the subtraction might not be defined, the latter because the gradients live in different tangent spaces so that the subtraction is not meaningful. We extend the idea in [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF] to the case of constrained optimization: with z ∈ T x (k) M, the step from x (k) to x (k+1) , we define

s (k) = T z z (22)
y (k) = ∇ z L (k+1) (0, λ (k+1) ) -T z ∇ z L (k) (0, λ (k) ) (23)
where T z is a vector transport along z, mapping a vector of T x (k) M to a vector of T x (k+1) M (see Fig. 4), see [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF] for more details. This way, all quantities live in T x (k+1) M and we can perform meaningful operations. The resulting Ĥ(k+1) can be viewed as the matrix of a quadratic form from

T x (k+1) M × T x (k+1) M to R.
We also use the usual damped update to account for the fact that we have a constrained optimization, and define r (k) as a modified y (k) :

θ (k) = 1 if s (k)T y (k) ≥ 0.2s (k)T H(k) s (k) 0.8s (k)T H(k) s (k) s (k)T H(k) s (k) -s (k)T y (k) otherwise r (k) = θ (k) y (k) + 1 -θ (k) H(k) s (k) with H(k) such that for u ∈ T x (k+1) M, H(k) u = T z ( HT -1
z u). We tried several update schemes: the BFGS formula

Ĥ(k+1) = τ (k) H(k) - H(k) s (k) s (k)T H(k) s (k)T H(k) s (k) + r (k) r (k)T s (k)T r (k)
with τ (k) = 1 for the original formula [START_REF] Nocedal | Numerical Optimization[END_REF], or

τ (k) = min 1, s (k)T r (k) s (k)T H(k) s (k)
for the so-called self-scaled version [START_REF] Nocedal | Analysis of a self-scaling quasi-newton method[END_REF]; and the SR1 formula [START_REF] Nocedal | Numerical Optimization[END_REF]

Ĥ(k+1) = H(k) - (y (k) -H(k) s (k) )(y (k) -H(k) s (k) ) T (y (k) -H(k) s (k) ) T s (k)
Finally, we tried a mixed BFGS-SR1 scheme [START_REF] Fletcher | A new low rank quasi-Newton update scheme for nonlinear programming[END_REF], but we did not get good results with it. The above formulas approximate directly the Hessian of L. Another approach is to use them to approximate individually the Hessian matrices of the cost function f and of every constraints c i , before using the fact that [START_REF] Goldsmith | Sequential quadratic programming methods based on indefinite hessian approximations[END_REF]. We refer to this as the individual update, and coin the first approach grouped update. The different update options are evaluated on robotics problems in Section VI.

∇ 2 xx L = ∇ 2 xx f + i λ i ∇ 2 xx c i , as proposed in
For most of the update schemes, the Hessian approximation Ĥ need not be positive definite. We regularize it as follows to obtain a positive definite approximation Ĥ : we perform first a Bunch-Kaufman factorization P ĤP T = LBL T where P is a permutation matrix, L is unit lower triangular and B is block diagonal with blocks of size 1 × 1 or 2 × 2 3 , see [START_REF] Golub | Matrix computations[END_REF]. We can then compute (in O(n)) the eigenvalue decomposition B = QDQ T , with Q a block diagonal orthogonal matrix with the same structure as B, and we define the diagonal matrix D with diagonal elements d ii = max (d ii , µ min ) where the d ii are diagonal elements of d and µ min > 0 is user-defined. We then define L = LQD 1 2 , and Ĥ = P T L L T P . Ĥ is positive definite because it is similar to D whose eigenvalue are all strictly positive (Sylvester's law of inertia). Computing L is cheap (O(n 2 )) given the sparsity of D and Q.

It is not uncommon for QP solvers to accept directly the Cholesky factorization (P, L ), as they would otherwise perform it internally. This is the case for the solver LSSOL [START_REF] Gill | User's guide for lssol (version 1.0): a fortran package for constrained linear least-squares and convex quadratic programming[END_REF] we use in this work. As a consequence, the overhead cost of the regularization is barely noticeable.

E. Overall algorithm

The full SQP algorithm is depicted in Fig. 5.

Proving formally the convergence of the algorithm is out of the scope of this paper. Intuitively, it behaves locally like an SQP. In particular, when close enough to the solution, the algorithm will take steps equal or very close to those taken by a classical SQP with a fixed local parametrization centered at the solution (i.e. problem [START_REF] Gabay | Minimizing a differentiable function over a differential manifold[END_REF] formulated at the solution x * ), because the effects of the change of local parametrization become negligible. Thus the algorithm should have the same convergence properties as a classical SQP.

IV. POSTURE GENERATOR Writing a PG can easily become cumbersome without the appropriate tools: many data and functions need to be specified and there are many sources of errors. Common pitfalls are for example writing the derivative of a function, managing how the Jacobian matrices of the already implemented functions are modified when a variable is added to the problem, adding a new type of constraint, or correctly writing a function on a sub-manifold of the problem's manifold. A fair amount of bookkeeping is always necessary, which should not be the charge of the user. We propose a PG architecture automating most of the problematic tasks so that the user can focus on the mathematical formulation of the problem. 3 obtaining B as a diagonal matrix is not numerically stable for Choleskylike decomposition of indefinite matrices

A. Geometric Expressions

The description of a task (i.e. a robotic constraint) is a mathematical object called expression. An expression is the result of a series of elementary operations on or between geometric and physical objects, called features, such as vectors, points, wrenches or frames. Provided that the elementary operations, features, and expressions are well-defined, and that the structure of the series of operations is registered in a system of expression graph, one can systematically compute the resulting expressions' values and derivatives by chain rule. The main idea behind such a framework is to separate the purely mathematical logic layer from the geometric one, in order to automate the evaluations and differentiations of complex operations between the geometric features.

The evaluation of a given feature in a given frame is represented by a mathematical expression. We consider 5 types of expressions which can be either variable or constant:

• Scalar, a 1-dimensional element of R • Coordinate, a 3-dimensional element of R 3
• Rotation, a 3 × 3 matrix representing a 3D rotation • Transformation, a 4 × 4 matrix representation of a 3D isometry • Array, a dynamic size array These expressions can be combined with each other through mathematical operations to create other expressions. Meaningful unary (norm, inverse, opposite...) and binary (addition, subtraction, dot product, cross product...) operations are implemented, along with their derivatives by chain rule. In addition, we have a Function class for more complex and custom expressions. In particular, we use a function operation expressing q → T i (q) where T i is the transformation between the reference frame of the robot and the frame of its i-th body to represent the location of each body of a robot 4 . We also use the combinations of elementary operations to define the computation graph used for evaluations.

The expressions and operations constitute the mathematical layer, on top of which the geometric layer is built. The geometric layer is composed of geometric and physical objects used in PG problems, which can be represented by elementary geometric quantities, or features. It aims at abstracting the mathematical layer so that the user can focus on the physical formulation of the problem. Any feature is defined in a reference frame by a mathematical expression but otherwise exists independently of its expression in a given frame.

The geometric layer of our framework in composed of 4 features, all defined in their respective reference frames:

• A Frame, defined by a Transformation expression;

• A Point, defined by a Coordinate expression;

• A Vector, defined by a Coordinate expression;

• A Wrench, defined by a pair of Coordinate expression. A special World Frame object serves as the starting reference frame. Since each frame is defined with respect to another reference frame, the geometric layer intrinsically constructs a tree of interconnected frames. This structure allows computing the numerical evaluation of any geometric object in any given frame. It allows the geometric layer to ensure that all the features are expressed in the correct frames before the mathematical layer performs the corresponding operations on their associated expressions. Basic operations are defined between those features (when applicable). For example, the subtraction between two Points gives a Vector. The geometric logic resides in the change of frame and those operations.

Let us consider a robot that has to look at a specific point P e of the environment with its camera, defined by a point P r and a vector V r . Using the elementary operations of subtraction between points (returns a vector), and the dot product between vectors (returns a scalar), the constraint writes as (P e -P r ).V r = 0. The user needs only to create those geometric objects, attached to the frames of the robot or of the environment, and write the code (Pe-Pr).dot(Vr) to generate the function. Given any variable object v, (Pe-Pr).dot(Vr).diff(v) returns the differential of the expression w.r.t. v. This makes the writing of constraints very easy.

B. Automatic mapping

The complete manifold M on which the optimization takes place is the Cartesian product of all the sub-manifolds involved in all the functions describing the optimization problem, and so is its representation space E and the optimization vector x ∈ E:

M = M 1 × M 2 × M 3 × . . . E = E 1 × E 2 × E 3 × . . . x T = [x T 1 , x T 2 , x T 3 . . .] (24)
4 The kinematics of rigid body systems is handled by the RBDyn library (https://github.com/jorisv/RBDyn)

The solver does not 'see' the different sub-manifolds. It considers all functions as defined on the complete manifold M, represented by E. Conversely, for the developer, it is quite inconvenient and a potential source of error to write a function on the complete manifold because of the need to manage indexes in order to feed the function with the correct data from the complete optimization vector x, and also because the complete manifold M can only be formed once the PG problem has been completely defined. Thus, when the function is implemented, the complete E is not known. A userwritten function f is usually defined on a subspace of E, say E I = E i × E j × E k . . . which is minimalist for that function, and should not account for unrelated manifolds. Our automatic mapping tool keeps track of all the necessary mappings for each function added and upon the instantiation of the problem, generates the correct projection functions π I : E → E I such that the developer can write a function f on E I while the solver receives it as a function f • π I on E. This idea is illustrated by the example in Fig. 6 Fig. 6. Automatic variable mapping: example of a function f taking values from E 5 × E 2 and written as such, which is mapped into a function from E 1 × . . . × E 6 .

C. Problem Generator

The problem generator is in charge of building the optimization problem for each PG problem to solve. It registers all the variables and constraints of the problem as well as the cost function. Each function is likely to bring additional variables with it. For each contact contributing to the static equilibrium, a variable defined on (a subspace of) R 6 representing its wrench is added to the problem. An additional function on that variable can be added for the slippage avoidance constraint. Once the registration is finished, the complete manifold on which the optimization takes place is generated as the Cartesian product of all the unique sub-manifolds on which the functions' variables are defined. The information of the automatic mapping is then used to 'plug' each function with the correct sub-manifold. Hence, the optimization problem can be generated and passed to the solver. The communication between the solver and the generated problem is made through the RobOptim framework 5 .

The previous tools simplify the development of PG problems and help to avoid implementation mistakes, greatly facilitating the implementation of new or more complex constraints.

V. EXTENSIONS TO NEW CONSTRAINTS

In this section, we first leverage the variable management capabilities of our PG and propose a constraint formulation that let the the optimization solver chose the contact location on a surface by using of additional variables. We also discuss the formulation of constraints and cost functions involving forces and torques and their implication: the need to compute the derivative of the joint torques, for which we propose an algorithm that does not rely on the computationally expensive differentiation of point's Jacobians.

A. Contact location parametrization

The most common type of contact constraint is between two surfaces S 1 and S 2 . Let F i = {O i , (x i , y i , z i )} be a frame defined on each surface S i , with O i on the surface and z i its outbound normal. Writing relations between geometric quantities defined in frames allows describing a large variety of contacts, such as planar or fixed contacts. For example, a planar contact between S 1 and S 2 can be expressed as:

       ---→ O 1 O 2 • z 1 = 0 z 2 • z 1 ≤ 0 z 2 • x 1 = 0 z 2 • y 1 = 0 (25)
This formulation is valid if the physical entities in contact are either a fixed point or a flat surface, because the quantities in eq. 25 are invariant w.r.t the location of the contact points. With non-flat surfaces, the locations of the frames used in the formulation matter if the contact points are not fixed. Taking advantage of our framework, we propose to parameterize the location of the contact points with an additional variable u S ∈ M S , whose manifold will automatically be added to the optimization problem. This gives our solver the ability to autonomously choose an optimal contact location on a nonflat surface as part of its resolution process. Depending on its shape, the manifold M S on which the surface is parametrized can be R, R 2 , S 2 . Then we can write the contact constraint on a frame parametrized as follows:

F(u S ∈ M S ) = {O(u S ), (x(u S ), y(u S ), z(u S ))} (26)
This approach can readily formulate contacts with surfaces represented by closed-form equations, such as spheres, superellipsoids. . . as presented in [START_REF] Brossette | Humanoid posture generation on non-Euclidean manifolds[END_REF] and in section VI-C1.

Additionally, in [START_REF] Escande | Parametrization of catmullclark subdivision surfaces for posture generation[END_REF] we present a method using a Catmull-Clark subdivision surfaces along with a custom raycasting algorithm to systematically generate a smooth parameterization of the surface of any object described by a star-convex mesh. This opens the way to a large range of applications of contacts in robotics. We illustrate it briefly in Section VI-C2.

B. Force Constraints

If we command an overall posture in which a robot link (e.g. tool used) is asked to apply a threshold of force in a given direction, it must be computed to meet such a requirement. One may also want to limit a force on a given contact or figuring if a robot is able to carry an object of a given weight (see Section VI-D1). Forces can be defined by using a wrench element in our expression framework. In many cases, satisfying the Newton-Euler equation is considered to be enough to have a static equilibrium of a posture, but when constraints on forces are involved, it becomes necessary to check the torque limits. Indeed, without torque limits, if we consider a robot pushing on two opposite walls, a configuration where the forces applied on each wall are opposite and arbitrarily large can be a solution, although not feasible in practice.

Having constraints on forces in our PG makes it necessary to compute the joints' torques, as well as their derivatives.

C. Joint Torque Jacobian Matrix Computation

The computation of the joint torque's Jacobian matrices is often approximated using finite differences, automatic differentiation, or some complex algorithms computing the derivatives of point's Jacobian matrices explicitly. We propose an algorithm that computes the exact torque Jacobian matrices.

For a given configuration q and a set of external wrenches w ext i , the joint torques can be computed recursively by using a modified version of the recursive Newton-Euler algorithm presented in [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF], where the time derivative terms are ignored.

Let us introduce some notations:

• n J and n B : respectively the numbers of joints and bodies • B i and J i : respectively the body and the joint of index i • pred(i) and succ(i): respectively the index of the predecessor and successor bodies of J i • j X i , j R i and j t i : respectively the transformation from the reference frame of B i to body B j , its rotation part, and its translation part • ∧: the cross-product operator • For any 3D vector v: v, the 3 × 3 skew-symmetric matrix such that ∀u ∈ R 3 , vu = v ∧ u • I i : the inertia matrix of B i • S i : the motion subspace of J i • a g = a c a f : the acceleration field, with a c its rotation part, and a f its translation part

• w ext i = m ext i
ξ ext : the external wrench applied on B i , with the moment part m ext i , and the resultant part ξ ext • dim(.): the dimension of . For any joint J i , the transformation from B pred(i) to B succ(i) denoted X PtS i = succ(i) X pred(i) can be decomposed into a static transformation X x i from the reference frame of the predecessor body to the one of the joint, and a variable transformation X J i (q), q being the joint parameter, such that X PtS i = X J i (q)X x i . Let A and B be the Cartesian frames with origins O A and O B respectively, t be the coordinate vector expressing ----→ O A O B in A, R be the rotation matrix that transforms 3D vectors from A to B coordinates. The transformation from A to B for a motion vector and its inverse are defined as follows:

B X A = R 0 -R t R ; B X -1 A = A X B = R T 0 tR T R T (27)
The transformation from A to B for a force vector, and its inverse are defined as follows:

B X * A = R -R t 0 R ; B X - * A = R T tR T 0 R T (28)
The torque computation is done in two steps: (i) the generalized forces on all bodies are computed, then (ii) the effect of each body on its parent is propagated through the joints and the torques are computed by Alg. 1. We differentiate this The column j of the Jacobian matrix of a body i represents the derivative of its rotation i R W and translation i t W w.r.t q j , that we denote respectively ω i,j and v i,j .

Algorithm 1 Inverse Static Algorithm ∀i ∈ [0, n B ], ξ G i = I i i X 0 a g -i X * 0 w ext i for i = n J -1 : 0 do τ i = ξ G i T S i ξ G pred(i) = ξ G pred(i) + X PtS i (q) - * ξ G i end
Jac W i = ω i,0 • • • ω i,j • • • ω i,dof v i,0 • • • v i,j • • • v i,dof (29)
To differentiate the Inverse Static algorithm, we first expand all the matrix products of its equations, and then differentiate each term. The following relations [START_REF] Lawrence | User's guide for CFSQP version 2.5: A C code for solving (large scale) constrained nonlinear (minimax[END_REF] are useful when differentiating Alg. 1, into Alg. 2.

∂ i R W u ∂q j = i R W u ∧ ω i,j = i R W uω i,j ∂ i R W i t W ∧ u ∂q j = i R W v i,j ∧ u + i R W i t W ∧ u ∧ ω i,j = -i R W uv i,j + i R W i t W u ω i,j (30)
Alg. 2 allows to compute the exact Jacobian of the torques in a robot without having to compute the derivatives of the forces application points.

VI. EVALUATION AND EXPERIMENTATION A. Solver's evaluation: the cubes stacking problem

In a typical robotics PG problem, the only non-Euclidean manifold encountered is the instance of SO(3) representing the 3D rotation of each robot's free-flyer. The other variables (joint angles and contact forces) can be represented in realspaces. Because of the predominance of Euclidean manifold, and the complexity of the problem's equations, it is unlikely for a formulation on manifolds to converge to the same local minimum as a formulation on real-spaces, and if they do, the influence of the formulation should be minor. Thus, in order to compare those two formulations, we consider a toy problem involving many non-Euclidean manifolds and simple equations:

Algorithm 2 Torque Jacobian Matrix Computation
Compute the Jacobian matrix of generalized forces

for i = 0 : n B do ξ G i = I i i X 0 a g -i X * 0 w ext i M =   i R W a c 0 -i R W i t W a c + i R W a f i R W a c   N =   -i R W i t W ξ ext i + i R W m ext i i R W ξ ext i i R W ξ ext i 0   fJac W i = Jac W i 0 6×dim(y) ∂ξ G i ∂x = (I W M -N )fJac W i + i X * 0 ∂w ext i ∂x end for
Compute the Jacobian of torques

for i = n J -1 : 0 do fS i = S i 0 6×dim(y) ∂τi ∂x = fS T i ∂ξ G i ∂x h i = R J i T ξ G i K =   (R J i T m G i ) + h i • t J i T -h i T • t J i I 3 -h i h i 0   ξ G pred(i) ← ξ G pred(i) + X P tS i (q) - * ξ G i ∂ξ G pred(i) ∂x ← ∂ξ G pred(i) ∂x -X x i K fS i end for
stacking a set of unit cubes in a box defined by a set of planes, with no interpenetration. Each cube is defined on R 3 ×SO(3). For any cube C i , we denote V i = {v 0 , v 1 , . . . , v 7 } the set of all its vertices, t i ∈ R 3 its translation and R i ∈ SO(3) its rotation w.r.t the world frame. A plane P j = {d j , n j } is described by its normal n j ∈ S 2 , and by d j ∈ R, its signed distance to the origin along n j . The constraint for a cube C i to be 'above' a plane P j is of dimension 8 and can be written as:

∀v ∈ V i , ( t i + R i v) • n j ≥ d j (31)
For the cubes to be inside a box defined by a set of planes P, we constrain each cube's vertex to be above each plane of P.

To avoid interpenetration, we could use classical collision avoidance constraints, but the use of the exact mesh of the cubes would generate gradient discontinuities of these constraints. Approximating the mesh with STP-BV [START_REF] Escande | A strictly convex hull for computing proximity distances with continuous gradients[END_REF] would allow avoiding those discontinuities. Yet, the closer to the exact mesh is the STP-BV approximation, the closer to discontinuity is the gradient. Instead, we propose another approach that uses non-Euclidean manifolds: for each pair of cubes (C i , C j ), we require a plane P ij = {d ij , n ij } to separate them. To do so, we constrain C i to be above P ij , and C j to be above [START_REF] Wächter | On the implementation of a primaldual interior point filter line search algorithm for large-scale nonlinear programming[END_REF].

P - ij = {-d ij , -n ij }, both using constraint
To simulate gravity, we minimize the potential energy of all the cubes (simplified by a factor mass times gravity):

f = i t i • z (32)
We consider the problem of stacking n cubes in an open-top box composed of 5 planes (the ground and 4 walls). In Fig. 7, we illustrate the case where n = 3 cubes. There is n(n-1)/2 separating planes, one per pair of cubes. Thus the search manifold is:

M = R 3 × SO(3) n × R × S 2 n(n-1) 2
The problem contains 5 constraints of dimension 8 per cube to fit them in the box and n(n-1)/2 constraints of dimension 16 to avoid interpenetrations. We have a problem of dimension 4.5n + 1.5n 2 with 32n + 8n 2 constraints. We also formulate this problem over R n to compare the resolution with and without the use of manifolds. Each variable on SO(3) is replaced by a variable on R 4 , while each variable on S 2 is replaced by one on R 3 . In both cases, a norm-1 constraint on the variable is added to the problem to force those variables on the manifolds. This results in a problem on

M = R 3 × R 4 n × R × R 3 n(n-1) 2 = R 5n+2n 2
that is a problem of dimension 5n + 2n 2 with 32.5n + 8.5n 2 constraints, which is n(n+1) 2 more variables and constraints relatively to the manifold formulation.

We solve these two problems for different numbers of cubes and compare the results in terms of number of iterations before convergence, convergence time, and time spent per iterations. In each resolution, both problems (Real space and Manifold formulations) start with the same initial guess. The resolutions are carried out with our solver, PGSolver, for both formulations with the same set of parameters, and with an off-the-shelf SQP solver, CFSQP [START_REF] Lawrence | User's guide for CFSQP version 2.5: A C code for solving (large scale) constrained nonlinear (minimax[END_REF], for the Real-space formulation only, using CFSQP default parameters. The initial positions of the cubes are chosen randomly, and each plane is initialized at a position between the two cubes it separates.

The results of these tests are plotted in Fig. 8. With 300 resolutions per case, about 98% converged when using manifolds versus 99.5% for non-manifold using PGSolver. The success rates of CFSQP drop incrementally from 100% for 2 cubes to 70% for 7 cubes. Concerning the resolutions with our PGSolver, we observe that the numbers of iterations are sensibly similar for the two types of resolutions. Yet, the time spent per iteration is consistently smaller in the case of manifolds, which is in agreement with our expectations. Subsequently, the convergence time is consistently shorter for the formulation with manifolds. The resolutions with CFSQP take 4 times more iterations and each iteration is on average 3 times longer than with our PGSolver. Resolutions with PGSolver on manifolds are on average 7 times faster than with CFSQP on Real-space for this specific type of problem.

This study shows that on a toy example, optimization on manifolds with our PGSolver not only allows the user to benefit from a simple and more intuitive formulation of the problem, but it also outperforms the classical approach (using CFSQP) in terms of success rates and convergence speed.

B. Posture Generator's evaluation

We tested our PG in different types of feasible problems, and compare the success rates and computation times of different resolution approaches. To generate feasible problems, we compute locations reached by the robot's end effectors (feet and grippers) for a random configuration q rand within its joint limits. We then compute different PG problems to find configurations with 2 (feet), 3 (feet and right gripper) and 4 (feet and grippers) contacts at the aforementioned locations. In particular, the contacts on the foot are fixed, unilateral, and with friction constraints (i.e. contact forces must be within friction cones), whereas the contacts on the grippers are fixed bilateral contacts (i.e. forces can be applied in any direction). Those contact constraints are formulated with the equations [START_REF] Goldsmith | Sequential quadratic programming methods based on indefinite hessian approximations[END_REF], whose quantities are all functions of the robot's variables q ∈ M. If we were using a framework with optimization on Euclidean spaces, those functions would need to be composed with a projection on ψ(M ) and additional constraints would need to be added to the problem, as explained in II-A. We attempt a resolution for each PG problem, using the q rand as initial guesses. If a solution q * is found (the problem is feasible), it is saved to be used in the test.

Yet, using q rand to generate the problem results in very twisted configurations of the robot that do not reflect 'useful' postures. We computed another set of feasible problems in which the value of q rand is taken closer to the half-sitting posture: q rand ← 1 2 (q rand + q half-sitting ). This resulted in better success rates and postures, which implies that the initial guess and its distance from the solution, that we name the initial distance, impact the resolution of PG problems. We define the Fig. 9. Comparison of success rates and convergence times of the PG resolution w.r.t the distance between the initial guess and the solution for sets of feasible problems with 2, 3 and 4 contact constraints and for different choices of Hessian update methods in our solver, and also with the IPOPT solver [START_REF] Wächter | On the implementation of a primaldual interior point filter line search algorithm for large-scale nonlinear programming[END_REF].

initial distance for a problem as d = q 0 -q * 2 , where q 0 is the initial guess and q * is a solution.

In order to highlight the effect of the initial distance on the success rate and convergence time of our PG, we solve the feasible problems several times, with an initial guess iteratively closer to a solution. For a random initial guess q 0 , we initialize the problem with q n% 0 = nq 0 + (1 -n)q * , with n taking successively the values 1.0, 0.8, 0.6, 0.4, and 0.2.

We compare the results obtained with three types of Hessian updates, namely BFGS, BFGS with self-scaling and SR1; for each, we use grouped and individual updates in restoration. In addition, we solved those problems with the framework proposed in [START_REF] Bouyarmane | On autonomous humanoid robots: Contact planning for locomotion and manipulation[END_REF], that formulates the same problems on realspaces and uses the IPOPT solver [START_REF] Wächter | On the implementation of a primaldual interior point filter line search algorithm for large-scale nonlinear programming[END_REF]. We gather the results after solving 250 different PG problems, 5 times each, starting increasingly close to the solution. We sample the range of initial distances into 20 segments of same length and report the average results on each segment as a data point in Fig. 9.

Although no Hessian update method clearly dominates the others, we can observe that for the most constrained problems, with 3 and 4 contacts, the BFGS with self-scaling methods present the best success rates but are slower than other methods, whereas for problems with only 2, the BFGS method outperforms the others. Running such experiments allows the user to devise strategies to tune the solver optimally for a given family of problems. Here, it suggests that for solving PG problems in which only the feet of the robot are used for contact, an update of type BFGS should be used, while for problems in wich hands also play a role in contacts, a BFGS with self-scaling is preferable. In no cases did the rank-1 update SR1 provide the best results.

Our framework arguably performs better in all cases w.r.t the framework of [START_REF] Bouyarmane | On autonomous humanoid robots: Contact planning for locomotion and manipulation[END_REF] that we denote IPOPT. With 2 contacts, the success rates are similar, but the convergence times of IPOPT are much longer. With 3 and 4 contacts, the convergence times are similar, but the success rates of IPOPT are much lower, averaging around 25%. This shows that our PG framework is more efficient and robust w.r.t the state-of-the-art.

These results also show that with our solver, the closer the initial guess is to the solution, the more likely a solution is to be found, and it gives us a quantitative estimate of the expected success rate and convergence times with respect to the initial distance. Interestingly, this observation cannot be made for resolution with the IPOPT framework.

Although all those computation times are long, and make it inconvenient to compute large numbers of postures, as is often done in contact planning, they remain of the same order of magnitude as the ones reported in [START_REF] Brossette | Point-Cloud Multi-Contact Planning for Humanoids: Preliminary Results[END_REF] (3-4 seconds per posture), [START_REF] Escande | Planning support contactpoints for humanoid robots and experiments on HRP-2[END_REF] (0.2 seconds per posture with only inverse kinematics). The most relevant comparison is with [START_REF] Bouyarmane | On autonomous humanoid robots: Contact planning for locomotion and manipulation[END_REF] that we presented earlier. Previous approaches are based on off-theshelf solvers, while we use an open solver that we developed and that can be further customized to become faster and more robust with further developments.

In order to have a better understanding of its most common failure cases, we compiled the termination status of our solver that we report in Table I. This allows us to evaluate the weaknesses of our solver and will guide our future developments toward improving its robustness. Having most of the failure cases related to reaching the maximum number of iteration is not abnormal, but some cases are linked to a slow convergence due to poor Hessian approximation. The failure of the SOC phase should not happen so often, thus we will need to investigate that aspect of the solver first.

C. Postures with contacts on parametrized surfaces

We present some use cases for parameterizing the location of a contact point on a given surface by adding variables to the PG optimization problem, Section V-A.

1) Contact with an object defined by a parametric equation: We want the HRP-4 robot to carry a cube in its two hands. Instead of running combinatorial cases of hands/faces contacts, we approximate the cube with a superellipsoid parametrized on S 2 . Its implicit equation is S(x, y, z) = 0, with

S(x, y, z) = x A r + y B r t r + z C t -1 (33)
A point in S 2 is represented by a vector v = (x, y, z) in E = R 3 . To a given unit v we associate a point αv on the surface of the superellipsoid by solving S(αv) = 0 for α. At this point, the normal is given by ∇S(v) ∇S(v) . Given this parametrization, we can write a contact constraint between given robot hand's frames and the point and normal on the parametrized superellipsoid's surface. The Fig. 10 presents some results for PG problems where the HRP-4 robot is required to grasp a cube modeled as a superellipsoid. On the left side, the feet are free to move on a sphere, on the right side, the left foot's position is fixed and the right foot is free to move on the ground. In both cases, contacts must occur between predefined points on the hands of the HRP-4 and points free to move on the surface of the cube. The cube is free to move (parametrized on R 3 ×SO(3)) and all the contact forces must ensure grasp and posture equilibrium.

This approach extends to any other 3D shapes that can be represented by parametric or implicit equations.

2) Contact with an object defined by a mesh: We now show an example of a PG using the 3D modeling technique proposed in [START_REF] Escande | Parametrization of catmullclark subdivision surfaces for posture generation[END_REF] to generate contacts with complex objects described by a 3D mesh. The Fig. 11 illustrates the HRP-4 robot climbing on a stack of cubes using both its hands and feet under constraints of static equilibrium, torque limits, etc.

The stack of cubes is modeled as a single object and the location of the 4 contact points are predefined on the robot but are free to be anywhere on the surface: the optimization algorithm determines the contact location as part of the solution. This is a very attractive feature of our approach as it allows to include discrete choices directly into our PG. This can be used to alleviate the work to be done by the user, be it a human or a planning algorithm: the user still has to specify the bodies in contact, but part of the combinatorics relative to the matching of a body with a particular surface or object is handled directly by the PG. Since the stack is a non-convex object, it cannot be used 'as is' in our collision detection algorithms. Thus, for collision constraints, we model it with a set of 21 slightly smaller cubes, see Fig. 11 (Right). The optimization takes around 115 iteration to converge to a solution.

D. Postures with force constraints

Tasks where the robot needs to apply a given set of forces or torques on the environment are necessary for many applications. Therefore, we need to complete our PG with 'forcebased' constraints to compute subsequent robot configurations.

1) Applying a desired force: To illustrate this feature, let's consider the example of a humanoid robot asked to apply a desired force on a given point of an airplane structure (printing or gluing a bracket, preparation for drilling, etc.). We denote ξ d the desired value of the force in direction d, and ξ c the actual force at the contact point. We can implement the following function f target (ξ c ) to minimize (hence a cost):

f target (ξ c ) = (ξ c • d -ξ d ) 2 (34)
In addition to that cost function, the robot needs to keep its foot on the ground, its left hand is used to lean on a beam of the structure to allow a wider reach with the right hand. Those constraints must be fulfilled under the joint and torque limits of the robot, maintaining balance and avoiding autocollisions. The result generated by our PGSolver is depicted in Fig. 12, where the robot applies a force of 100N in the desired direction on the airplane structure 6 .

We assessed the validity of postures generated this way by solving a similar problem with an HRP-2 robot, and executing it on the real robot in a mockup of the airplane structure. The robot had to apply 100N and 200N on the environment. The posture and contact locations computed by our PG are not enough to generate the desired effort on the right hand of 6 www.comanoid.eu the robot. Indeed, for a given posture, an infinite number of contact forces can be generated by modifying the joint torques, so a specific control strategy needs to be implemented: we controlled the right gripper in admittance, while the other contacts were simply controlled in position. The robot was able to reach those forces (100N and 200N ) without falling or slipping and while maintaining a posture very close to the one computed by our PG (less than 2% difference per joint w.r.t the joint range).

2) Applying a desired torque: We can also compute a posture where a desired torque is applied, for example in a torquing scenario pulling a lever by applying a minimum torque on its revolute joint. We consider a situation where a lever is attached to a wall of the environment by a revolute joint, the system {wall + lever} is considered as a robot with a single degree of freedom. We write constraints allowing the hands of the robot to be in contact with the lever, rotating and translating freely along it (the translations being limited by the length of the lever). Additionally, those constraints allow the generation of contact forces in the plane orthogonal to the lever's axis passing through the contact point. To enforce the value of the torque to generate on the revolute joint of the lever, the torque limits of the lever robot can simply be overwritten. This formulation, augmented by the usual constraints on this system, allows computing postures where a desired torque is applied on the lever. The Fig. 13 depicts a PG solution to such a case; the location of the hands and the contact forces are autonomously chosen by the PG to generate a torque of 20N.m on the revolute joint at the lever basis.

Note that inequalities can also apply to contact forces or torques to limit or threshold them if necessary.

VII. CONCLUSION

Computing feasible postures using nonlinear optimization on manifolds proved to be elegant in terms of problems specifications while keeping good performances w.r.t Real-space formulation of the same problem. We devised our own SQP solver on manifolds to deal more efficiently with robotic problems. Our posture generator software includes many features not found in existing inverse kinematics (even generalized) ones. For example, we added the force dimensionality (specifying task desired force/torque thresholds, directions, limits, etc.) and static equilibrium constraints (grasp and whole-body positioning). We also show that we treat surfaces defined by closed-form equations or star-meshes to leave the decision of the contacts localization to the solver on manifolds. We also conducted assessment test on toy examples and illustrate the scalability and applicability to humanoid robots 7 .

There are still things to improve, most of which are rather technical and require engineering efforts in software development (e.g. a user-friendly interactive API). On the research part, we will work on deeper specialization of the solver to the PG problems. It would be interesting to run more tests to study the influence of different solver options and find optimal strategies to set the solver's option to solve PG problems, maybe using learning techniques. Perhaps choosing automatically the update method and other options based on the structure of the problem at hand would allow increasing the general performance.

APPENDIX SO(3) EXAMPLE OF NON-CONVERGENCE

Given x the angle-axis representation of a rotation (rotation of angle x and axis x), let R(x) be the corresponding rotation matrix. Given a rotation R t , let us consider min.

x -trace(R T t R(x))

whose solution is attained for x such that R(x) = R t . Taking the starting point x (0) = 2π -0 0 T and R t = R( 0 0 T ) state-of-the-art solvers do not converge for small enough (e.g = 0.01 with Matlab's fminunc). This is because R(x (0) ) is very close to R t so that the gradient of the objective is small but x (0) is far from 0 0 T .

Fig. 1 .

 1 Fig. 1. Illustration of the different constraints involved in the Posture Generation problem.

Fig. 4 .

 4 Fig. 4. The vector v 1 ∈ Tx 1 M is transported along z. Tx 1 ,z (v 1 ) is an element of Tx 2 M and can be compared to other elements of this tangent space such as v 2 .

← ρ 2 Fig. 5 .

 25 Fig. 5. Main SQP Loop

Fig. 7 .

 7 Fig. 7. Stacking 3 cubes in a box, separating each pair of cubes by a virtual plane. Initial (left) and final (right) configurations.

Fig. 8 .

 8 Fig. 8. Comparison of resolutions with and without using manifolds: red curves are the results with PGSolver on manifolds; blue dashed curves are those of the Real space; the green thick curves are with CFSQP on Real space.

Fig. 10 .

 10 Fig. 10. HRP-4 carrying a 2kg cube. Left: feet on a sphere, objective function is to maintain the cube at a given position. Right: right foot free to move on the floor, objective is to put the cube as far as possible in a given direction.

Fig. 11 .

 11 Fig. 11. HRP-4 climbing a stack of cubes modeled as a single object with a single surface (left). The convex shapes used for collision avoidance (right).

Fig. 12 .

 12 Fig. 12. HRP-4 / TORO applying a 100N desired force on a contact point with its right hand.

Fig. 13 .

 13 Fig. 13. HRP-4 applying a 20N.m desired torque on a lever.

  for algorithm w.r.t. q and any other variables of the problem that we denote generically y (e.g. force variables, parametrization of non-flat surfaces, configuration of another robot, etc). We assume that the derivatives of m ext i and ξ ext i :

	and	∂m ext i ∂q , i , all the quantities of ∂m ext i ∂y , ∂ξ ext i ∂q , i and ξ ext ∂y are given. Besides m ext ∂ξ ext i
	the algorithm depend only on q.

In practice, we use only the derivatives of the Lagrange function, so that λ -and λ + are irrelevant, and we work with λ = λ + -λ -. Since for i = 1 • • • m, (λ -) i = 0 or (λ + ) i = 0 depending on whether the upper or lower bound of the i-th constraint is active, λ -and λ + are obtained from λ.

http://www.roboptim.net/

Software available at https://github.com/stanislas-brossette/manifolds and https://gite.lirmm.fr/posture generator/problem-generator

the reviewer's comments. This work was partially supported by the EU H2020 COMANOID project and the CNRS-AIST-Airbus Joint Research Program.

S. Brossette is with Wandercraft,