Adrien Escande

Abderrahmane Kheddar

Sylvain Miossec

Planning contact points for humanoid robots

Keywords:

We present a planner for underactuated hyper-redundant robots, such as humanoid robots, for which the movement can only be initiated by taking contacts with the environment. We synthesize our previous work on the subject and go further into details to give an in-depth description of the contact planning problem and the mechanisms of our contact planner. We highlight the structure of the problem with a simple example, present the contact space and the heuristics we use for planning, and explain thoroughly the implementation of the different parts of the planner. Finally we give examples of planning results for complex scenarios with the humanoid robot HRP-2.

Introduction

Underactuated systems such as humanoid robots can only move by contacts (and sometimes grasps) with the environment. In an obstacle-free, flat, horizontal or lightly-sloped terrains a humanoid robot moves by alternating contacts between the feet and the ground. Each contact creates a constraint on a foot and the reconfiguration of the posture by the actuators induces a movement of the remaining links. For such environments, several approaches proved to be efficient to plan motion for humanoid walking [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF][START_REF] Takenaka | Real time motion generation and control for biped robot-1st report: walking gait pattern generation[END_REF][START_REF] Herdt | Online walking motion generation with automatic foot step placement[END_REF][START_REF] Herdt | Walking without thinking about it[END_REF] or even running [START_REF] Takenaka | Real time motion generation and control for biped robot-2nd report: running gait pattern generation[END_REF][START_REF] Takenaka | Real time motion generation and control for biped robot-3rd report: dynamics error compensation[END_REF][START_REF] Takenaka | Real time motion generation and control for biped robot-4th report: integrated balance control[END_REF]. The seminal work in [START_REF] Kalisiak | A grasp-based motion planning algorithm for character animation[END_REF] illustrates motion generation from contact prints for general biped computer graphics figures. More recently impressive walking behaviors are generated automatically [START_REF] Yin | SIMBICON: simple biped locomotion control[END_REF][START_REF] Muico | Contact-aware nonlinear control of dynamic characters[END_REF] even for highly uneven virtual terrains [START_REF] Mordatch | Robust physics-based locomotion using low-dimensional planning[END_REF][START_REF] Coros | Generalized biped walking control[END_REF][START_REF] Wu | Terrain-adaptive bipedal locomotion control[END_REF] (see also [START_REF] Sentis | Motion planning of extreme locomotion maneuvers using multi-contact dynamics and numerical integration[END_REF] in robotics). In cluttered environments, specific motion planners are devised. Situations where the humanoid is asked to cross under or over obstacles are tackled respectively by [START_REF] Yokoi | Unified motion planning of passing under obstacles with humanoid robots[END_REF][START_REF] Verrelst | Dynamically stepping over obstacles by the humanoid robot HRP-2[END_REF]. In another work, a dedicated approach deals with situations where crossing under the table or moving through a narrow tunnel are asked [START_REF] Kanehiro | Whole body locomotion planning of humanoid robots based on a 3D grid map[END_REF]. In the latter case, crawling using knees and elbows is the most appropriate strategy for humanoids. In other studies [START_REF] Lu | Walk-to-brachiate transfer of multi-locomotion robot with error recovery[END_REF], planning and control of dynamic transition between walking, swinging with hands, and crawling for Gibbon (monkey-like) robots illustrates the need to use several links for locomotion.

There is however the need for more general planners. One could gather the above different approaches into contact postures and * Corresponding author. Tel.: +81 298619244. contact transitions primitives, whose combination could be used to plan motion for various situations. But it is difficult to unify these approaches because they use different control strategies and they require tremendous efforts and tricks for tuning. Even if such primitives could be devised, each of them would be valid for predefined links and for a predefined contact spot for each link. Ideally, in cluttered environments, a humanoid robot must be capable of planning motion without restricting the choices of possible contacts that can be made, i.e. considering contacts between any part of its body and any parts of the environment (and even its own body). This is what we are tackling here. Working with contacts is not straightforward: contacts are taken on the surfaces of objects that are also obstacles we do not want the robot to collide with. They also occur for subspaces of the configuration space with zero measure. For these reasons, classical planning algorithms cannot be used readily. Our work is a solution to that problem. It proposes a planning algorithm that achieves general non-gaited multi-contact planning. It combines an adapted best first planning algorithm (BFP), to span a tree of possible contacts and a posture generator that checks the feasibility of a contact configuration. The BFP algorithm is entirely revisited to plan in the space of contacts and the posture generator is formulated as a non-linear optimization problem. To our best knowledge, our planner is to this day the only one that has been successfully applied to humanoid robots (or systems of equivalent complexity) in complex, cluttered environments, such as presented in the two last scenarios of this paper.

Our planner belongs to the class of contact-before-motion planners in that the obtained whole-body trajectory is the consequence of a sequence of contacts (whereas in the motion-beforecontact planners, the contacts are chosen after a trajectory has Fig. 1. Left: a 2d tripod robot with 2-dofs legs and single point contacts. Right: a representation of the 8-dimensional configuration manifold of the robot, with the 7 submanifolds corresponding to the different contact combinations. Note that each submanifold's representation corresponds to a different projection so as to depict all of them in a 3d drawing which captures the connectivity of these spaces. been planned). Earlier works in this class include the planner of Bretl [START_REF] Bretl | Multi-step motion planning: application to free-climbing robots[END_REF] for climbing robots that has been later adapted to humanoid robots by Hauser [START_REF] Hauser | Non-gaited humanoid locomotion planning[END_REF]. The contacts happen at fixed predefined locations in the environment, sometimes sampled at random on surfaces in the latter. Due to the use of RRT, the output plan is jerky and needs some smoothing in post-processing, that cannot eliminate all unnecessary movements, hence a lack of naturalness. This lack is due both to the use of RRT and the fixed locations of the contacts that might not be suitable. In [START_REF] Hauser | Using motion primitives in probabilistic sample-based planning for humanoid robots[END_REF], motion primitives are used to get smoother paths and choose more natural contacts. The solution is however limited by the library of primitives, losing some generalness. By taking another algorithmic approach, our planner does not suffer these drawbacks. The contacts can be taken anywhere on predefined surfaces and both the postures and the choices of contacts are obtained by minimizing a cost function. This enforces naturalness while keeping generalness. Moreover our approach lets the user have some control on the overall look of the solution, as will be detailed later. The contribution of this paper is twofold: (i) we illustrate the difficulties of contact planning by an example with a simplified robot, (ii) we propose an algorithm for the general case and give a detailed and comprehensive description of all its components along with the justification of the heuristics used and the implementation details, going far beyond the synthesis of our previously published work [START_REF] Escande | Planning support contact-points for humanoid robots and experiments on HRP-2[END_REF][START_REF] Escande | Planning support contact-points for acyclic motions and experiments on HRP-2[END_REF][START_REF] Escande | Contact planning for acyclic motion with tasks constraints[END_REF][START_REF] Escande | Planning contact supports for acyclic motion with task constraints and experiment on HRP-2[END_REF].

The paper is organized as follow: we illustrate the contact planning structure (Section 2) then give a detailed description of our contact planner with a presentation of both the concepts (Section 4) and the implementation (Section 5 for a first basic yet complete presentation of the different parts, Section 6 for posture generation, and Section 7 for advanced planning modules). We then exemplify its usage with scenarios involving the humanoid robot HRP-2 [START_REF] Kaneko | IEEE International Confer[END_REF].

Problem overview

In this section, we illustrate the structure of the contact points planning problem. We consider a simplified 2-dimensional tripod robot evolving on a flat floor (at the elevation y = 0). It is reduced to a central point and three legs, each of which has a hip and a knee joint (see Fig. 1, left). The feet are considered to be single points. The configuration space of this robot C can thus be described by the 2D position of the mass, and two angles per leg (for which we do not consider any joint limits). C is thus an 8-dimensional manifold:

C = R 2
× S 6 (with S the unit circle). To model an underactuated robot, we consider that this robot can only move when at least one of its feet is in contact with the floor. Furthermore, the robot's movement is limited to lie in C free defined as the set of configurations for which all points P of the robot except the feet in contact must have an elevation y P > 0. Finally, we consider nonsliding contacts. This is a purely geometric approach since we do not account for the gravity field so that there is no equilibrium issue.

Contact submanifolds

A foot contact is geometrically specified by a 2-dimensional constraint: the position (x i , 0) of the foot on the floor. When a single foot F i (i = 1, 2 or 3) is in contact at (x i , 0), the robot evolves on a 6-dimensional submanifold S i x i of C free which is the null-space of the constraint. We also consider the 7-dimensional submanifold S i = 

x i ∈R S i x i , which is the set of all configurations for which F i is in contact, regardless of contact position. Likewise, we define the 4-dimensional submanifolds of C for two feet F i and F j (i ̸ = j) respectively in contact at x i and x j (we drop the always-zero elevation of the contact for conciseness) as D i,j x i ,x j . These sets are bounded by the constraints on x i and x j induced by the robot geometry: the feet cannot be further apart than the legs lengths allow. Thus D i,j

x i ,x j ⊂ ∂S i x i ∩ ∂S j x j
, where ∂X denotes the boundary of X .

From there we obtain the 5-dimensional manifolds D i,j

x j =  x i ∈R D i,j x i ,x j ⊂ ∂S i ∩ ∂S j x j and D i,j x i =  x j ∈R D i,j x i ,x j ⊂ ∂S i x i ∩ ∂S j , as well as the 6-dimensional submanifolds D i,j =  (x i ,x j )∈R 2 D i,j x i ,x j ⊂ ∂S i ∩ ∂S j .
The first one is the set of configurations for which the foot F j is fixed as a specified position x j while F i has the correct elevation, but its position on the floor is unspecified (yet within the geometrical limits). The second one is the symmetric case where F i is fixed and F j is not. D i,j is the set of configurations where both F i and F j are contacting with the floor without being fixed to it. Lastly, we can define a 2-dimensional submanifold for triple contacts T x 1 ,x 2 ,x 3 , with F 1 , F 2 and F 3 in contact at x 1 , x 2 and

x 3 , and all the 3,4,5-dimensional manifold obtained by relaxing the constraint on the position of one or several feet, among

which is T =  (x 1 ,x 2 ,x 3 )∈R 3 T x 1 ,x 2 ,x 3 .
The submanifolds S i , D i,j and T represent the 7 possible contact combinations of the robot in our example, with respectively one, two or all three of the feet F i in contact (see Fig. 1).

{F 1 } , {F 1 , F 2 } , {F 2 } , {F 1 , F 2 } , {F 1 } , {F 1 , F 2 } , {F 2 } , {F 2 , F 3 } , {F 3 }, {F 1 , F 3 } , {F 1 }.
The movements in double support phases cannot be represented due to the lack of dimensions of the 3d representation.

Movement limitations

For a given i,

 S i x i  x i ∈R
is a family of disjoint manifold. Mathematically it forms a foliation of S i , and a submanifold S i x i is called a leaf. Such structures have already been exhibited in manipulation planning [START_REF] Siméon | Manipulation planning with probabilistic roadmaps[END_REF][START_REF] Hauser | Multi-modal motion planning for a humanoid robot manipulation task[END_REF]. In terms of movements for the robot, two distinct leaves correspond to two distinct placements of F i , and thus it is not possible to go directly from one leaf to another: that would mean a contact sliding. The movements in S i are thus restricted to happen in the leaf the robot is currently in. To move from one leaf to the other, the robot will need to go out of S i , which means making (at least) another contact.

Likewise,

 D i,j x i ,x j  (x i ,x j )∈R 2 (resp.  D i,j x i ,x j  x i ∈R ,  D i,j x i ,x j  x j ∈R ,  D i,j x i  x i ∈R and  D i,j x j  x j ∈R ) is a foliation of D i,j (resp. D i,j
x j , D i,j x i , D i,j and D i,j ) for which it is not possible to go from one leaf to the other. In each case, it means that in order to go to another leaf of the same foliation, the robot must travel out of this foliation, which corresponds to adding or removing a contact. Similar structures and conclusions arise in the triple contact case, see Fig. 2.

The complexity of contact planning

Due to the structure of the configuration space and the restriction on the robot movements, planning a path with contacts is much more complex than the classical piano mover's planning problem where we search a path in a continuous space. The contact planning problem, on the contrary, is a hybrid problem which mixes three kind of choices. Firstly, when the robot is in a given contact combination, there is a discrete choice to be made concerning the next combinations: if it is in S 1 for example, the robot can attempt to add a contact and go either to D 1,2 , D 1,3 or simultaneously two contacts and go to T . From D 1,2 , it could go to S 1 , S 2 or T , by removing a contact in the two first cases, or adding one in the last case. Secondly, when adding a contact, there is a continuous choice to be made as to where the contact should occur, i.e. what will be the leaf of the next submanifolds. Lastly, within a leaf, we compute the path between the point where the robot enters the leaf and the one planned for its exit.

Several difficulties occur in more complex situations: the presence of obstacles and the need for configuration taking into account the stability of the robot reduce the feasible space, so that it might not always be possible to find a path on a leaf between the entering and exiting configuration. There can also be several contact surfaces, increasing the number of combinations for single, double and triple contact submanifolds, since not only the choice of the feet in contact must be made, but also the pairing of each contact foot with a surface.

To summarize, contact planning features three kinds of choices:

• the discrete choice of the sequence of contact combinations, i.e. a sequence of foliations,

• the continuous choice of the positions of the contacts, i.e. the choice of a leaf within a foliation, and

• the continuous choice of a path on a leaf between two contact combinations.

This triple level of choices is particularly complex because of the huge size of the set of discrete choices and because each of the choices is highly dependent on all the other choices previously made. In particular, the continuous choice of a leaf at a time will impact the further possibilities of discrete choices. The addition of physics considerations such as the equilibrium of the robot adds complexity to the problem and prevents us from straightforwardly using the reduction property used in [START_REF] Siméon | Manipulation planning with probabilistic roadmaps[END_REF].

Best first planning

The contact planner we are presenting in this paper is derived from a potential-field-based planner called Best First Planning (BFP) [START_REF] Latombe | Robot Motion Planning[END_REF], briefly describe hereafter.

Algorithm

The main principle of BFP is to grow a tree of configurations based on a discretization of the configuration space C by expanding at each iteration the best leaf of the actual tree, where best is defined by a potential function over C. The root node is the configuration q init , and the expansion of the tree ends when one of the new leaves is (very close to) the target configuration q end . The expansion of a leaf relies on the notion of m-neighbor in a discretized space: if δ is the discretization step, a m-neighbor of q is obtained by (separately) incrementing or decrementing at most m coordinates of q by δ. For instance, if C = R 2 and δ = 0.1, the 2-neighbors of (1, 0) are (0.9, 0), (0.9, -0.1), (1, -0.1), (1.1, -0.1), (1.1, 0), (1.1, 0.1), (1, 0.1) and (0.9, 0.1).

The algorithm is described in Algorithm 1. T is a tree whose nodes values are configurations, and L is the list of its leaves, i.e. the nodes that have not been visited for expansion yet. L is sorted in increasing order, according to the potential function U. Two operations are defined on T :

• add(q, T ) adds a node with value q to T , • isAlreadyIn(q, T ) checks if T has a node with value q. Data: q init , q end ,U(q) Result: a sequence  q 0 , q 1 , • • • , q l  , with q 0 = q init , q l = q end and q i+1 son of q i for 0 ≤ i < l 1 -L is a sorted list, initially empty 2 -T is a tree, initially empty 3 begin 4 insert(q init ,L) 5 add(q init ,T ) 6 while no isEmpty(L) do 7 q ← first(L) 8 for each neighbor q ′ of q do 9 if U(q ′ ) < M and no isAlreadyIn(q ′ ,T ) then 10 setFather(q ′ , q) 11 add(q ′ ,T ) 12 insert(q ′ ,L) 13 if q ′ = q final then 14 return backtrackPath(q ′ ) 15 return failure Algorithm 1: Best First Planning Algorithm For the list structure, three operations are available:

• insert(q, L) inserts a leaf with value q in L according to its potential value U(q),

• first returns the first element of L, i.e., the leaf with smallest potential U(q), and deletes it from the list,

• isEmpty(L) returns true when the list L is empty, false otherwise.

Finally, setFather(l, n) assigns n as the father node of the leaf l, and backtrackPath(q) goes back from father to father from the node with value q to the root of T and returns the path from the root to q.

By always expanding the best non-visited node of T , BFP acts as a steepest descent algorithm with predefined possible descent direction (the choice of m for the m-neighbors), and fixed step length, until it reaches a local minimum of U. Then it consider every discretized configurations in the basin of attraction of this minimum by increasing order of potential value, until it finds a way out and resumes its steepest descent-like search. A physical image of this process would be water following a slope, arriving in a basin and filling it up until it goes over the edge and flows away from it.

Features

It is worth pointing out several features of the plain BFP algorithm, to see how they will translate when tackling the contact planning:

1. a potential field function U: it gives a guide for the search. This guide is however not perfectly followed (i.e. the algorithm does not descend following exactly the gradient) because of the way the sons of a node are chosen. In order to have the algorithm return a correct path, U should be an attractive field toward the target configuration, and be repulsive when nearing obstacles. 2. a discretization of the search space, on which the notion of neighbors relies and hence the way sons are generated. The discretization step is a crucial parameter of the algorithm. It must be small enough to ensure passing through corridor of the configuration space, yet big enough so as to not increase the computation time unnecessarily. 3. free path between neighbors: it is a strong hypothesis of BFP, that the path between two m-neighbors lies in the free space if both neighbors do. This hypothesis is most likely valid when the discretization is small enough. Otherwise line 9 of the algorithm should include an additional checking function freePath(q, q ′ ).

4. limitation of the search space: if U globally increases when going away from the target configuration, the choice of M defines a closed set of the configuration space. This is necessary for termination.

5. loop avoidance: the test isAlreadyIn(q ′ , T ) prevents the algorithm from visiting the same configuration twice. With 4, this ensures termination. 6. new leaves: the way to generate the sons of a node implies the direction the algorithm can take to perform the search. In BFP, the sons are the m-neighbors, but other choices could be made. 7. ending condition: the way to define the target was reached.

Here it is when a leaf is equal (or very near to) the target configuration.

Some of these features are trivial. They are nonetheless crucial and will get more complex in contact planning.

Contact planner: specificities and concepts

Contacts and planning space

From here, we consider a robot with a free-flying root and n unit joints. The particularity of contact planning is to look for a path in submanifolds of the configuration space corresponding to the surfaces of the C-obstacles: denoting r i (q) the volume of the workspace occupied by the i-th body r i of the robot at configuration q, R(q) the volume occupied by the whole robot, and O j the volume of the j-th object of the environment, the planning occurs in

C c =  q ∈ C|∃i, R(q) ∩ O i ̸ = ∅, R(q) ∩ • Oi = ∅ and ∀j ̸ = k, r j (q) ∩ r k (q) = ∅  where • X denotes the interior of X .
Not all points of C c are interesting however, because some surfaces of the environment or the robot may not be appropriate for contact: a window may not bear the robot's weight, some parts of the robot may be too fragile, etc. To account for this and to simplify the problem, we consider contacts between predefined spots of the robot and predefined surfaces of the environment. We also restrain to fixed contacts (e.g. non-sliding or non-rolling).

A contact spot on the robot is defined by a triple

 r i , H i j , h i j 
where r i is a body of the robot, H i j is an element of SE(3) expressing the position of a contact frame with respect to the body's frame, such that the origin of this frame is a point of the body's surface, and the z-axis of this frame is an inner normal to this surface. h i j is a set of points forming the convex hull of the contact surface. Note that for a body r i we can define several contact spots. The set of all contact spots is noted S contacts .

For the environment, we select smooth surfaces that can support a contact and denote their set S surfaces . For each surface, it is possible to define a continuous mapping giving the normal vector and a tangent one for each point (x, y). For the sake of simplicity, we will consider only planar surfaces, but our algorithms extends easily to the surfaces of general convex objects by taking correctly into account the contacting area between the body and the surface, and less trivially to non-convex cases by carefully considering the collisions.

A robot-environment contact c is fully determined by the choice of a couple of S contacts × S surfaces and the triple (x, y, θ ), where (x, y) denotes a point of the environment surface, and θ is the angle around the normal vector at (x, y) made by the tangent vector at (x, y) and the x-axis of the contact frame of the robot. The choice of the couple being discrete, the set of all contacts c is a subset of the 4-dimensional space N × R 3 . For a given k, we consider the k-tuples of contacts such that no body appears more than once, and we denote S k the corresponding set. k must be at least one, because the robot needs at least one contact to be stable, and cannot be more than n + 1 since a robot with n unidimensional joints and a free flying root has n + 1 bodies. Consequently, we define the set

S = n+1  k=1 S k . (1)
This set of sets of contacts appears to be the natural planning space for our problem: moving a robot from one place to another amounts to searching for a sequence of sets of contacts achievable by the robot. In this space, a m-neighbor of an element is another set of contacts obtained by at most m contact changes (addition

or removal, but not displacement: going from (c 1 , c 2 ) to (c 1 , c ′ 2 ),
where c 2 and c ′ 2 involve the same body, is done in two steps by first removing c 2 then adding c ′ 2 ).

Feasible sequence of contacts sets

We define here what is an achievable sequence for the robot.

S is more meaningful for our purpose than C and is smaller since effectively the number of contacts is less than 4 or 5. The size of S is thus less than 20 while the size of C ranges usually from 30 to more than 100. Yet, we cannot forget completely about C because we need the configuration of the robot to test some physical and geometrical properties. We say that a set of contacts s ∈ S is feasible if there exists (at least) one posture for the robot, such that the contacts are effective and the geometrical constraints (joint limits, collision avoidance. . . ) and physical constraints (equilibrium, possible torque limits. . . ) are satisfied. Such a posture is a witness posture. We denote by Q s the set of all witness postures for a contacts set s. Whether it is empty or not can be decided by a feasibility problem (FP), that we describe in Section 6.

A sequence of feasible contacts sets (s 1 , s 2 , . . . , s l ) is feasible itself, if there is a continuous path satisfying successively each s i :

∃ϕ : [1,

l] → C, (1) ϕ is C 0 , (2) ∀i ∈ [|1, l -1|] , ∀u ∈ [i, i + 1[, ϕ(u) ∈ Q s i (3) ϕ(l) ∈ Q s l .
Finding such a path might be done by using classical planning methods on constrained submanifolds [START_REF] Cortés | Motion planning algorithms for general closed-chain mechanisms[END_REF][START_REF] Stilman | Task constrained motion planning in robot joint space[END_REF]. These approaches however are too costly to be used as checking routines within our planner; therefore we use a heuristic we now describe.

For ϕ to be continuous, we need two conditions: (i)

Q s i ∩Q s i+1 ̸ = ⊘ and (ii) Q s i-1 ∩ Q s i and Q s i ∩ Q s i+1 both belong to the same connected component of Q s i .
The first condition means that one can find a path to change the contacts from s i to s i+1 . We first remark that Q s i can be written as the intersection of two subsets of C, one defined by the stability (i.e. equilibrium) constraints derived from s i that we denote Q stab s and the other defined by all the remaining (and mostly geometric) constraints,

Q geom s . (i) implies that Q geom s i ∩ Q geom s i+1 ̸ = ⊘.
A sufficient way to ensure this is to plan only with 1-neighbors, i.e. one passes from s i to s i+1 only by adding or removing exactly one contact. Let us assume that, without loss of generality, s i ⊂ s i+1 , which implies Q geom

s i ⊃ Q geom s i+1 and Q stab s i ⊆ Q stab s i+1 .
We then have:

Q s i ∩ Q s i+1 ̸ = ⊘ ⇔ Q geom s i ∩ Q stab s i ∩ Q geom s i+1 ∩ Q stab s i+1 ̸ = ⊘ (2) ⇔  Q geom s i ∩ Q geom s i+1  ∩  Q stab s i ∩ Q stab s i+1  ̸ = ⊘ (3) ⇔ Q geom s i+1 ∩ Q stab s i ̸ = ⊘. (4)
This means that there is a path between two 1-neighbors s i and s i+1 if one can find a posture satisfying the geometric constraints of the biggest (for the inclusion) of the two sets and the stability constraints of the smallest one. Condition (ii) means we can find a path within Q s i to perform the contact changes from s i-1 to s i+1 . In some specific cases (for example with a thin obstacle) this path may not exist, but these cases being rare, we suppose there is always one.

We thus construct our feasible sequence by adding or removing contacts, one at a time, and finding at each step a witness posture taking into account the changing contact geometrically but not physically (i.e. for the stability). This can be seen as checking the feasibility just before a new contact is made or just after an existing one was released.

Contact planner: implementation

This section discusses the implementation of our contact planner by describing its different components. We give a first practical way to implement each of them with the exception of the Posture Generator, in charge of the feasibility checking, which is described on its own in the next section. This first implementation gave good results in not-too-complicated scenarios [START_REF] Escande | Planning support contact-points for humanoid robots and experiments on HRP-2[END_REF]. Section 7 goes into the details of more sophisticated implementations for some of these components, aiming at tackling more complex environments and achieving better speed. Since we work in S but need to prove the feasibility of a contact set by a witness posture, the input and output of the algorithm are based on couples from S × C. A node can then be seen as a triple (f , s, u) with f the father node, s ∈ S and q ∈ C. The goal, determining the successful termination of the algorithm is given as a subset Q goal ⊂ C, possibly of lower dimension, described as a set of equalities and inequalities on q. It is reached for the set of contacts s e if a posture q e exists in Q s e ∩Q goal . To guide the planning toward the goal we also provide the algorithm with a potential field U defined over S × C (although one might want to use only S or C).

Contacts BFP

Algorithm: Contacts BFP Data:  s init , q init  , Q goal ,U(s, q) Result: a sequence  s 0 , q 0  ,  s 1 , q 1  , • • • , (s e , q e )  , with  s 0 , q 0  =  s init , q init  , q e ∈ Q goal and s i+1 1-neighbor of s i 1 -L is a sorted list,
Unlike the classical approach in potential field planning, U cannot incorporate readily a repulsive component based on the distance between the robot and the environment since at any time, parts of the robot need to be in contact with the environment. The robot however needs to be guided around (or under/over) the obstacles. We introduce in Section 7.2 a way to build a potential field that copes with this obstacle/support duality. For the sake of simplicity, we will write U(n) for U(s, q), where n = (f , s, q). Likewise we

write Q n for Q s .
The algorithm still grows a tree by expanding the best leaf according to U, that can be found in the sorted list L. The function allowsToReachGoal(n) checks whether Q n ∩Q goal is empty or not by trying to solve the corresponding feasibility problem, and returns true upon success, producing a witness posture q e . We suppose that the path between n • q and q e exists. generateSons(n) generates the sons of n and is detailed in the next section. It includes what is equivalent to the loop avoidance, discretization and leaves generation in the classical BFP. Lastly, we mention the function trajectoryExistsTo(n) which returns true if a path exists between n and its father. In our implementation, this function is trivial and always returns true, since we follow the heuristic mentioned above.

Sons generation

Algorithm: generateSons

Data: n, a node Result: l, the list of leaf expanded from n, initially empty The second test is the feasibility test. generate(n, c) with n = (f , s, q) attempts at finding a witness posture q w for the non-emptiness of the set Q s∪{c} with c not participating to the stability. This is the transcription of the sufficient conditions of (i) in 4.2. Note that in Algorithm 4, c is already in s, yet it will not participate to the stability. generate is implemented as a feasibility (or optimization) problem as described in Section 6. The process is initialized with the actual configuration q attached to n and replaces it by q w upon completion. This is why in both Algorithms 3 and 4 we construct the new nodes with the configuration of their fathers: the sets Q attached to the father and the sons being largely similar, the father's configuration already satisfies many constraints of the sons' feasibility problems. The second part is more complex because there is usually an infinite number of possible new contacts. This complexity is handled by the function generateNewContacts presented in Algorithm 4 and its sub-function getContactCandidates. This second part iterates over the possible new pairs in S contacts × S surfaces .

The sub-function isAlreadyInContact(s, i) returns true if the body corresponding to the i-th contact spot in S contacts is already participating to a contact in s. Remember that the sets of contacts in S cannot involve twice the same body of the robot.

The working of Algorithm 4 is fairly straightforward. Two subfunctions need to be explained: forbiddenContact(i, j) relies on a list of invalid pairs of S contacts × S surfaces given by the user and returns true if (i, j) denotes a pair in this list. The use of this list can be motivated by security issues, the control of the overall aspect of the result or the introduction of some a priori knowledge. For example, one might want to avoid the robot putting its feet on a table. getContactCandidates(i, j) returns a list of potential contacts between the contact spot i and the surface j, that have then to be validated by the block we described above. How to build this list is discussed in Section 5.4.

Ordering S

A contact can be described by the tuple (i, x, y, θ) with i the index identifying a couple in S contacts ×S surfaces . For s ∈ S, we define ind(s) = [i 1 , . . . , i k ] the sorted list of the indices corresponding to the contacts in s. We also construct p s = (x 1 , y 1 , θ 1 , . . . , x k , y k , θ k ), the vector of these contacts parameters, in the same order as the indices in ind(s).

If s and s ′ are such that ind(s) = ind(s ′ ), then we say that s and s ′ are equal iff ∥D (p s -p s ′ )∥ ≤ k • ϵ, with D a diagonal matrix to account for the non-homogeneity between angles and distances, and ϵ a positive constant. We write p s ̸ = p s ′ if the previous inequality is not verified. One must note that this equality relation is not transitive.

Using the lexicographic order on the vectors, we can then define an order relation ≺ on S :

s ≺ s ′ iff • card(s) < card(s ′ )
• or card(s) = card(s ′ ) and ind(s) < ind(s ′ )

• or ind(s) = ind(s ′ ) and p s ̸ = p s ′ and p s < p s ′ .

Through the use of the set N with the equality presented here, we obtain an implicit discretization of S whose parameters are D and ϵ: when a node n is inserted in N, it defines a region of S in which we cannot chose a new set of contacts (because the test add(n ′ , N) would fail). Provided the environment is bounded, we thus have a finite (yet huge) number of contacts sets. How these sets are distributed in S is determined by the way of generating contact candidates.

Generating contacts candidates

A pair of S contacts × S surfaces being given, generating candidates amounts to choosing triples (x, y, θ ).

Ideally, the generation of candidates should verify the three following points:

(i) ensure a good coverage of the surface, to avoid ignoring possible paths for the robot, (ii) give a small number of candidates to avoid combinatorial explosion, (iii) give feasible contacts as much as possible. Indeed, the feasibility checking is the most expansive part of the planner, and takes more time when failing to find a witness posture than when it manages to produce one.

Determining a strategy to take into account these three points is a crucial step of our contact planner. Such a strategy must be cheap enough so that it does not cost more its benefit, especially regarding point (iii). Reducing the contact surface we consider is a necessity and can be roughly done by intersecting the surface with a sphere centered at the root of the limb containing the contact spot and a conservative radius. Our first implementation of the candidates generation is based on this sphere-surface intersection and a random generation of (x, y, θ ) with (x, y) on the intersection.

The compromise between (i) and (ii) is done by choosing the number of generated triples. It appears experimentally that taking a number proportional to the square root of the surface's area gives good results: this gives us enough samples for small surfaces yet not too much for larger ones.

Potential field

Constructing a potential field efficiently guiding the planning is the second crucial point in our work. We discuss here some basic solutions and will present a more complex one in Section 7.2. The idea is above all to have a guide in the workspace: we do not aim at a precise posture, but usually at a place in space or more precisely, a subset of C. Thus, a first idea is to construct a simple attractive potential   xx 0   2 based on the position (and possibly the orientation) of the robot's root or CoM and a target position. For a given node, this potential will be evaluated at its witness posture. Another idea is to base this potential on the position of the contacts or their barycenter, and a target barycenter. Being defined only on S, this potential field has the advantage of not relying on the witness posture that is dependent of the way to produce it. These types of potential may be combined. We can also add to them bonus or penalty terms in order to favor or penalize some class of contacts sets, for motivation ranging once again from security to aesthetic. For example, when we wanted to run the output of the planner on a real robot, we greatly penalized the sets with hands only, to prevent the robot from walking on its hands, while permitting it to use them in conjunction with other parts, for instance to pass under a table.

An important drawback of the potential fields presented here is that they do not take obstacles into account. This will be tackled in Section 7.2.

Posture generation

This section describes a tool that is extensively used throughout the planning process, since it provides the witness postures attesting for the feasibility of a set of contacts. It relies on the notion of task, which denotes in this work a set of equalities and inequalities over C, namely g(q) = 0 and h(q) ≤ 0. A contact is a special case of task. The naming task was already used for equalities in a planning context [START_REF] Stilman | Task constrained motion planning in robot joint space[END_REF], and is closely related to the notion of task function in control [START_REF] Samson | Robot Control: The Task Function Approach[END_REF].

Feasibility problem

Given a set of tasks {T i }, the subset Q ∈ C of the geometrically and physically correct configurations verifying these tasks is written (see Fig. 3):

Q =            q - i ≤ q i ≤ q + i , ∀i ∈ [|1, n|],
(a)

ϵ ij ≤ d(r i (q), r j (q)), ∀(i, j) ∈ I auto , (b) ϵ ik ≤ d(r i (q), O k ), ∀(i, k) ∈ I coll , (c)
s(q) ≤ 0, (d)

g i (q) = 0, ∀ T i , (e)
h i (q) ≤ 0, ∀ T i (f) (5)
where inequalities (5)(a) describe the joint limits. We consider 1d joints. Bounds for more complex joints could be written in a more general form b(q) ≤ 0.

Eqs. ( 5 involved in a contact should not be part of it, for example. s is a stability criterion written only on the configuration q and will be described in Section 6.4.

One might want to take into account the limitation on the actuators of the robot. This can be done by introducing additional variables, namely the torques τ and the contact forces f. They are linked to q by the dynamic equation of a multi-body system in the static case:

 g 1 (q) g 2 (q)  =  τ 0  +  J T c 1 (q) J T c 2 (q)  f (6)
which should then be added as a constraint (J c being the Jacobian matrix of the contact points corresponding to f and g the gravity term). One can then impose bounds on τ :

τ - i ≤ τ i ≤ τ + i , ∀i ∈ [|1, n|]
and the stability constraint will be rewritten as s(f) ≤ 0 and accounts for the contact forces being inside friction cones. This solution however increases the number of variables, which induces a greater cost to solve the feasibility problem. We do not consider bounds on τ in this work.

Positioning constraints

Geometrically imposing the contact between a body r i of the robot and a surface of the environment is a special case of positioning a body at a given location in the workspace.

We denote H j i the element of SE(3) describing the position of a frame i w.r.t. a frame j. For a vector v (resp. a point p), we write v i (resp. p i ) its coordinates in the frame i and v j (resp. p j ) in the frame j. We have the non-linear relation

v j = R j i v i (resp. p j = H j i p i ), where R j i is the rotation part of H j i . If k is a third frame, we have H k i = H k j H j i
(see [START_REF] Duindam | Modeling and Control for Efficient Bipedal Walking Robots-A Port-Based Approach[END_REF]Chapter 2]). We denote the world frame by 0.

Positioning the body r i in the workspace is done by superimposing a frame attached to this body (we called it contact frame in Section 4.1) with a frame in the environment. Let i denote the local frame of r i , j the attached frame and e the environment frame. The superimposition constraint can be written H 0

j (q)  =H 0 i (q)H i j  = H 0 e . It is conveniently expressed in R 6 by t 0 j (q) -t 0 e = 0 (7) log   R 0 e  -1 R 0 j (q)  = 0 ( 8
)
where t is the translation part of H. For R a rotation of axis u and angle θ, we have log(R) = θ u.

For a contact task, the frame j is chosen as described in 4.1 and e is a frame such that its origin is on an environment surface, and its z-axis correspond to the outer normal of the surface at this point.

For more modularity, we implemented the rotation part ( 8) by expressing the perpendicularity between the axes of frames j and e:

i 0 (q) • b 0 = 0 (9) i 0 (q) • n 0 = 0 (10) j 0 (q) • n 0 = 0 (11) -j 0 (q) • b 0 ≤ 0 (12) -k 0 (q) • n 0 ≤ 0 ( 13
) with i j = d e = (1, 0, 0) T , j j = b e = (0, 1, 0) T , k j = n e = (0, 0, 1).
The two inequalities are necessary to discriminate between the four possible cases satisfying the equalities.

The advantage of such a set of constraints over Eq. ( 8) is the possibility to take a subset of it. We use later the following weaker positioning constraints:

 t 0 j (q) -t 0 e  • n 0 = 0 (14) i 0 (q) • n 0 = 0 (15) j 0 (q) • n 0 = 0 (16) -k 0 (q) • n 0 ≤ 0. ( 17
)
These constraints force the origin of frame j to lie in the plan x-y of frame e but not at a particular position, and the z-axis of j is aligned with the z-axis of e, with the rotation around this axis let free. For a planar surface described by e, it means the contact spot j (of body r i ) is in contact with this surface, but the parameters (x, y, θ) of this contact are not fixed.

Other geometric tasks can be described this way, see for example [START_REF] Escande | Contact planning for acyclic motion with tasks constraints[END_REF].

Collision constraints

To express that two objects O 1 and O 2 (bodies of the robot or parts of the environment) are not in collision, we use a signed pseudo-distance, i.e. a function d such that

 d(q) > 0 if O 1 (q) ∩ O 2 (q) = ∅ (a) d(q) = 0 if O 1 (q) ∩ O 2 (q) = ∂O 1 (q) ∩ ∂O 2 (q) (b) d(q) < 0 otherwise. (c) (18)
Additionally, the gradient of this function needs to be continuous for the optimization solver. 1 The Euclidean distance between the objects, extended to the penetration case by quantifying the depth [START_REF] Van Den Bergen | Collision Detection in Interactive 3D Environments[END_REF], is a suitable candidate for Eqs. ( 18)(a)-(c), and its gradient is continuous when both objects are convex and one of them is strictly convex. We presented in [START_REF] Escande | Continuous gradient proximity distance for humanoids free-collision optimized-postures[END_REF] a strictly convex bounding volume to ensure this gradient's continuity.

To compute the gradient of d, we extend the reasoning found in [START_REF] Lefebvre | Fast computation of robot-obstacle interactions in nonholonomic trajectory deformation[END_REF] to the case of two moving objects: if P 1 and P 2 are the fixed points of O 1 and O 2 that coincide with the witness points of d (i.e. the points realizing the distance) at configuration q, denoting p 0 1 (q) and p 0 2 (q) their coordinates in the world frame, we have ∂d ∂q

= 1 d(q)  p 0 1 (q) -p 0 2 (q)  T  ∂p 0 1 ∂q - ∂p 0 2 ∂q  . ( 19
)

Stability constraints

We give here two expressions for the stability constraint s(q). The simplest one is based on the classical projection of the CoM in the sustentation polygon [START_REF] Wieber | On the stability of walking systems[END_REF]: for a contact, the contact spot gives us a set of points h =  p j 1 , . . . , p j k  expressed in its contact frame j. When the contact is realized, we have H 0 j (q) = H 0 e , so that the contact points are at the positions H 0



. Now, if we have all l contacts, the sustentation polygon is the (2d) convex hull of all the projected contact points: conv

 π  H 0 e 1 h 1  ∪ • • • ∪ π  H 0 e l h l



, where π : R 3 -→ R 2 denotes the projection onto the x-y plane. We denote π i the m points of this convex hull indexed in the trigonometric order, and

n i =  0 1 -1 0  (π i+1 -π i ) (
with the convention π m+1 ≡ π 0 ), the outer normal to segment π i π i+1 . Then, G(q) being the CoM expressed in 1 Non-smooth optimization routines for problems with generic objective and constraint functions are difficult to find off-the-shelf and usually not as fast and perfected as state-of-the-art algorithms for smooth optimization. the world frame, we have:

s(q) =    [π 1 -π (G(q))] T n 1 . . . [π m -π (G(q))] T n m    . ( 20
)
This criterion is valid if all contacts are on the same horizontal plane. It is conservative if the contacts are on different horizontal planes (see [START_REF] Wieber | On the stability of walking systems[END_REF]). For both situations, this is the criterion we adopt because it is fast. In all other cases we use a second criterion, which is a static version of the one proposed in [START_REF] Garsault | Non-gaited dynamic motion with multi-contact transition[END_REF][START_REF] Barthélemy | Stability measure of postural dynamic equilibrium based on residual radius[END_REF] and similar to [START_REF] Bretl | Testing static equilibrium for legged robots[END_REF]: if we consider that τ is not bounded, then the first n lines of Eq. ( 6) can always be satisfied, and the robot is only constrained by the last 6 lines which are equivalent to the Euler-Newton equations: g 2 (q) = J c 2 (q) T f. Note that if the equation is expressed in the world frame, what we consider now, J c 2 is independent of the configuration. Furthermore, to avoid sliding, each contact force must be inside a Coulomb friction cone and so f is inside a product of cones: f ∈ F = ×C j . If we consider discretized friction cones, then F is also a discretized (or polyhedral) cone (in dim(f) dimensions) so that f can be written as a positive linear combination of rays: f =  α i ≥0 α i f i where the f i are deduced from the rays of the discretized friction cones.

It follows that g 2 (q) =  α i ≥0 α i J T c 2 f i , hence g 2 (q) belongs to a
(possibly open) polytope that is the 6d convex hull of the J T c 2 f i . Using a double description algorithm such as cddlib [START_REF] Fukuda | Double description method revisited[END_REF], we can get a representation of this polytope as a set of linear inequalities and so write s(q) = Ag 2 (q) + b. With further operations, this criterion can even be expressed in 3 dimensions [START_REF] Qiu | Human motions analysis and simulation based on a general criterion of stability[END_REF]. Computing the linear inequalities takes up to a few ms for several contacts, each with 4 contact points and 4-sided discretized cones.

Adding a criterion

If Q is not empty, it is generally a non-countable set. It might then be useful to specify a criterion o to be minimized, so as to select a particular posture. The feasibility problem becomes then a minimization problem

min q∈Q o(q). ( 21
)
An advantage to using a criterion is that the obtained witness posture is less dependent on the solver mechanisms and the initial guess (the optimization problem may however have some local minima, even if o is convex, because of the constraints). This is important when the potential field of the planning is based on q so that nodes are ''fairly'' evaluated. To this end using the planner's potential field as a posture criterion is a possibility we use later.

Otherwise, the criterion can have several forms, from maximizing the stability of the robot (by rewriting each line of the stability constraint as s i (q) ≤ w, and minimizing w) to notions of comfort posture (see e.g. [START_REF] Yang | Multi-objective optimization for upper body posture prediction[END_REF]). We first used the distance to a reference posture: ∥q -q * ∥ 2 which gives good results for simple scenarios [START_REF] Escande | Planning support contact-points for humanoid robots and experiments on HRP-2[END_REF].

In the next section, we propose a variant of a criterion based on the distance to a reference path s ∈

[0, 1] → q * (s) ∈ C: min s,q   q -q * (s)   2 . ( 22
)

Implementation details

To solve the above feasibility/optimization problem we use FSQP [START_REF] Lawrence | User's guide for CFSQP version 2.5: a C code for solving (large scale) constrained nonlinear (minimax) optimization problems, generating iterates satisfying all inequality constraints[END_REF]. Our posture generator is build over two parts as advocated in [START_REF] Bonnans | Numerical Optimization-Theoretical and Practical Aspects[END_REF] (Chapter 1): the algorithm part which consists in an object encapsulation of FSQP and knows nothing about the problem to be solved, and the simulation part, which we called Posture Calculator, that performs all posture-related computation at the request of the algorithm, and knows nothing about optimization (see Fig. 4).

The Robot class performs all the computations related to the kinematics of the robot, for instance the position of a point of the robot in the world frame, or its gradient. The Constraint class represents a function from R m into R (with m the number of variables) that can be considered either as constraint or objective function by the solver. It implements the different kinds of parametric function that we need to describe the problem. For example the collision avoidance constraint between the objects O i and O j computes a • d(i, j, q) + b where a, b, i and j are the parameters and d(i, j, q) = d(O i (q), O j (q)).

ConstraintsSet implements a group of constraints describing a task, for instance a contact or the stability criterion. It is meant to help the user by providing a higher level interface, so that he just has to give the task parameters, and the corresponding constraints are automatically generated.

Lastly, the Posture Calculator handles two functionalities: it ensures the sorting of the constraints according to FSQP requirements (non-linear inequalities first, then non-linear equalities. . . ) and it keeps up-to-date the stability and collision constraints according to the contacts added or removed.

The computations are made using lazy evaluation so as to avoid evaluating the same data twice.

Improving planning

Advanced contact generation

We saw that for a pair of S contacts × S surfaces a contact is specified by a triple (x, y, θ ). For this pair, the sets of all triples is a subset V of R 3 which can be seen as the product of the surface (or the domain of its parameters) and the segment [0, 2π ] (see Fig. 5).

Searching for contact candidates amounts to finding points in this volume, and the three points of Section 5.4 can be rewritten as:

(i) ensure a good coverage of V , (ii) give points not too close from each other, (iii) give points corresponding to feasible contacts as much as possible.

(iii) remains the difficult point: if s is the set of contacts we try to add a contact to, the projection of Q s onto V is far from being trivial so that biasing the search toward ''good'' points in V does not seem to be practical.

The solution we propose is to let (x, y, θ ) be chosen by the Posture Generator, rather than proposing candidates which may not be feasible: using Eqs. ( 14)-( 17) instead of Eqs. ( 9)-( 13) for a new contact involving r i constrains only 3 dofs of r i , and lets (x, y, θ )

free. Upon successful completion of the posture generation with this weaker form of contact, we have the parameters of a feasible contact. If the generation fails, then we assume no contact can be found for the current pair (body, surface). This assumption may not hold, because the Posture Generator does not guarantee to find a solution if there is one. However, it happens to be true in practice for the vast majority of cases since the optimization is started with the configuration of the father, hence not far from the solution.

Once a first point P = (x, y, θ ) has been found in V , we have to find another one, not too close from it (condition (ii)). To do so, the idea is to generate a new point in V \ V P where V P is a small volume centered around P, then, for this new point, exclude another volume from V and generate a new one, repeating these steps until the Posture Generator cannot find a solution. In practice, we want to work with convex search volumes to avoid introducing (more) local minima in the optimization scheme, and we furthermore restrain these volumes to have boundaries in θ of the form θ = constant. Such a volume can be described by the following inequalities:

A  t 0 j (q) -t 0 e  + b ≤ 0 (23) θ - ≤ θ (q) ≤ θ + (24)
where A and b describe the limitation of the surface (that we assume to be convex or divisible in convex pieces) and θ (q) is the angle between the projection of  t 0 j (q) -t 0 e  on the surface and the reference direction. The volume V P is chosen to be a parallelepiped of dimensions δl×δl×δθ , with δl and δθ two parameters that control the density of points. V \ V P is then divided in convex volumes of the same shape (see Fig. 5). In each of these volumes a generation will be attempted before further subdivision, until no solution exists. This leads to Algorithm 5.

The construcSearchVolume(j) method divides the surface with index j into convex pieces and builds the corresponding search volumes by computing their product with [0, 2π ]. generate(n, i, V ) performs a posture generation with the contacts of n and the weaker form of contact involving r i within the search volume V . retrieveContact(n, i, j) identifies the values (x, y, θ ) for the contact between the pair identified by (i, j) at configuration n•q and returns the contact. Lastly, divideSearchVolume(V , c) subdivides V \V P into convex search volumes, where P is the point corresponding to c, and returns these new volumes, if any (V \ V P eventually gets empty). The algorithm terminates when there is no more search volumes in which to find contacts.

Guide path and potential field

Using solely an attracting potential field to guide the planner toward the goal appears to be limited because the presence of obstacles will create local minima in C free whose size will be correlated to the size of the obstacles, and the number of contact spots. We thus need a field that guides the planning around (or above/under) the obstacles. We achieve that by constructing a descending valley-like shape potential field following a rough path in C (see Fig. 6). This rough path gives an idea of the overall trajectory without the detailed movements due to the contacts. We define it as a linear interpolation between a small numbers of key postures. It does not matter if the path thus obtained does not lay fully in C free . These key postures can be computed automatically [START_REF] Bouyarmane | Collision-free contacts guide planning prior to non-gaited motion planning for humanoid robots[END_REF] or given manually. In the latter case, it gives the

Algorithm: generateNewContacts2

Data: n, a node, i and j, indices of the contact spot and the surface Result: l, a list of leaves whose father is n, initially empty 1 -N is the set of the nodes already generated 2 begin 3 if forbiddenContact(i,j) then 4 return(⊘)

5 n ′ ← n 6 V ← construcSearchVolume(j) 7 while no isEmpty(V) do 8 V ← first(V) 9 if generate(n', i, V ) then 10 c ← retrieveContact(n',i,j) 11 n ′ ← (n, n ′ .s ∪ {c}, n ′ .q) 12 V ← V ∪ divideSearchVolume(V , c) 13 if add(n ′ ,N) then 14 l ← l ∪  n ′  15 return(l)
Algorithm 5: Contact generation with search volumes user a way to encode high level specification about the motion like going around or under the table, entering the house by the front or rear door. . . Let p : [0, 1] -→ C be a continuous path, and π : C -→ C root the projection onto the configuration space of the robot's root. We denote q ′ and p ′ the images of a configuration q and the path p by π . We can safely assume that π is bijective from p to p ′ : it would not be the case if p ′ was self-intersecting, which would imply the robot is coming back to a place he was before (all the movements in between were unnecessary) or if the posture was changing while the root is kept fixed. This second case can be easily avoided: it does not hurt to change a bit the root's position, the path being only approximate. Then, denoting q ′ p the closest point of p ′ from q ′ , we define q p = π -1 (q ′ p ), and write the potential field as

U p (q) =   q -q p   2 -α • l ′ (25)
where l ′ is the curvilinear coordinate of q ′ p along p ′ . The first term of this function is a pseudo-distance to the path and gives the valleylike shape; the second accounts for the distance traveled along the path, and gives the slope toward the goal. The α factor weights the contribution of the two terms.

If p is piecewise linear, this potential field however is not even continuous because the closest point q ′ p jumps when q ′ passes from the Voronoi region of a segment to the other, implying a discontinuity of l ′ . This can be partially corrected by ''rounding'' the angles of p: we replace each angle by an arc of circle (in dimension n + 6) for which we chose the maximal radius such as the distance to the original path is less than a given constant. As long as the distance to the modified path is less than the radii, U p is C 1 .

Two terms can be added to this potential field. First, from p, it is easy to derive guide paths p i for the bodies r i , and build potential fields U p i (q) based on the distance from the bodies to their guide paths. Adding such terms to U p for selected bodies (typically the feet and hands, sometimes the knees) enables the planner to anticipate the movements of these bodies which is useful when the robot needs to radically change the posture (for example to pass from walking to crawling) (see [START_REF] Escande | Planning contact supports for acyclic motion with task constraints and experiment on HRP-2[END_REF]).

Second, we can add a term U S over S, as devised in 5.5, to account for the characteristics of the set of contacts.

Very best first contact planning

Unless the planner gets stuck in a huge local minima, what should be avoided by the use of the previously proposed potential field if a solution exists, most of the nodes generated during the planning will never be visited for further expansion, and thus are generated to no avail. We present here a method to drastically reduce the number of generated nodes.

Suppose at first that we have an oracle capable to tell us at no cost for a node f which of its sons n will have the best rating according to the potential field. Then, when f is expanded, it suffices to generate n and to add it in the list of leaves L. Indeed, the siblings of n being worse than n they will never be selected for expansion as long as n has not been selected itself. When, at an ulterior iteration, n is selected, its best sibling n ′ has to be put in L because it might be better than all the other elements already in L. So, each time a node is selected for expansion, only its best son and its best sibling are generated and inserted in L (see Fig. 7), which greatly reduce the number of unused nodes generated. We call this principle Very Best First Planning.

Unfortunately, in the case of contact planning, we do not have such an oracle, but we have a semi-oracle in the sense that we are able to predict that some nodes will be better than some others without generating them. This is achieved by using the potential field of the planner as the objective function for the posture generation. In this case, the new contact found for a given search volume will yield a better node than the contacts found for the subsequent sub-volumes. This leads to a modification of Algorithm 2 we named Contacts VBFP, where the line 11: l ← generateSons(n) is replaced by

l ← generateBestSons(n) ∪ generateNextSiblings(n).
The method generateBestSons(n) tries to generates all the nodes obtained by removing one contact from n, and for each couple of S contacts × S surfaces , only the first node that would be obtained by Algorithm 5. We cannot indeed predict the ranking order between these nodes. generateNextSiblings(n) returns an empty list when n has been obtained by removing a contact. If n has been created by adding a contact c, then the method tries to generate the nodes for the same couple of S contacts × S surfaces and the sub-volumes issued from the volume in which c was generated.

We observed that this lazy-generation scheme leads to creating 3-5 times less nodes than in the CBFP algorithm, with a similar gain in computation time and memory.

Results

In this section, we present the output of our planner applied to a HRP-2 robot in three scenarios of increasing complexity (see Fig. 8): climbing a ladder, crawling through a narrow tunnel, and sitting at a table while carrying a glass. The computations are performed on a single thread on a Pentium 4 at 3.2 GHz, with 2 Go of RAM. 117 pairs of bodies (r i , r j ) are considered for autocollision, all pairs are considered for robot-environment collision, and additional analytical constraints on the joint angles are written for collision avoidance at the hips and the neck (they have been identified empirically).

Ladder scenario

We begin by taking a situation similar to one in [START_REF] Hauser | Non-gaited humanoid locomotion planning[END_REF], in order to get a comparison: the robot has to climb a ladder, about 2 m tall. To get as close as possible to the conditions of [START_REF] Hauser | Non-gaited humanoid locomotion planning[END_REF], we restrict the robot to use only its feet and one face of each hand, and fix the orientation of the contacts. We however let the contact occur at any place on the lower and upper grounds and on the rungs of the ladder. For this experiment, while all the support surfaces are horizontal, we use the general stability criterion presented in Section 6.4, the CoM projection criterion being too conservative for this case. The robot starts in front of the ladder and the goal is specified as a region (at the top of the ladder) in which the waist needs to be.

Our planner finds a path with 35 nodes in about 8.5 min which is only slightly slower than [START_REF] Hauser | Non-gaited humanoid locomotion planning[END_REF] where the possible contact placements are given beforehand. A total of 497 nodes were generated.

Tunnel scenario

In this scenario, we demonstrate the ability of our planner to cope with cluttered environments and particularly to enter in and evolve through a narrow space, which is usually recognized as difficult for planners: the robot, which is 1.54 m tall and 63 cm wide has to go through a tunnel 73 cm wide, 54 cm high and 90 cm long. The entrance is in front of its initial position, 35 cm above the ground. Ten contact spots are defined on the robot: the feet, the knees, the elbows, the side of each hand, and the top the pinches. Four key postures are given to define the guide path: standing up in front of the tunnel, on its hands and knees at the entrance and at the exit of the tunnel and standing up after. The goal is specified as a region in which the waist needs to be.

The solution is found in 56.2 min and consists in 101 nodes, for a total of 3656 nodes generated. The planning time is divided into three roughly equal parts: entering the tunnel, going through it and getting up.

Table scenario

In this third scenario, the robot has to go and sit at a table while carrying a glass full of liquid in its right hand. The planning problem is defined as follow: there are 3 objects in the environment: the floor, the chair and the table. Twelve contact spots are defined: the feet, the knees, the thighs, the side of each hand, and the top the pinches and three environment surfaces are considered for contact: the floor, the seat of the chair, and the top surface of the table. We forbid the (hand, floor), (foot, chair) and (foot, table) contact pairs. An additional task T glass is added to every posture generation to keep the glass vertical. Consequently, all contacts with the right hand are forbidden. The main difficulty of the planning is that the robot has to enter the narrow space between the table and the chair, space in which it cannot be in full upright position. In particular, while the space seems more open than in the previous scenario, it is more difficult to find a stable posture when evolving around the chair. The task T glass adds complexity to the posture generation and prevents the robot from using its right hand for locomotion. We manually give a rough path with 4 key postures as depicted in Fig. 9.

The result, depicted in Fig. 8 is a sequence of 69 nodes, obtained in about 3 h, after 3800 nodes have been generated. The robot quickly gets to the side of the chair, before taking some time to put its feet in front of the chair (after about 50 nodes). Once it has entered this narrow passage, it reaches the goal in about 20 nodes. When near the chair, the robot takes support on the chair and the table with its left hand. Experiments on a real HRP-2 robot of this scenario are reported thoroughly in [START_REF] Escande | Planning support contact-points for acyclic motions and experiments on HRP-2[END_REF][START_REF] Escande | Contact planning for acyclic motion with tasks constraints[END_REF].

Conclusion

Planning a general multipurpose and multi-contact motion for humanoid robots is possible by relaxing the contacts to occur with any part of the robot and any part of the environment. Our approach consists in building a tree that explores possible contact configurations thanks to a generalized inverse kinematics that is formulated as a non-linear optimization problem in order to account for robot limitations in terms of joints limits, static equilibrium, non-desired collision avoidance, etc. Several complex scenarios were successfully planned using our planner. Further extensions of our method are already under implementation and consist in generalizing it to more generic agents as well as unifying multi-contact locomotion and manipulation in a single planning framework for all the components (the posture generator in [START_REF] Bouyarmane | Static multi-contact inverse problem for multiple humanoid robots and manipulated objects[END_REF], the planner in [START_REF] Bouyarmane | Multi-contact stances planning for multiple agents[END_REF]). We also considered deformable environments [START_REF] Bouyarmane | FEM-based static posture planning for a humanoid robot on deformable contact support[END_REF]. This demonstrates that our algorithm foundations are viable enough to allow such extensions. As future work, we will need to reconsider the issue of scalability with respect to the number of agents and the complexity of the situations. The bottleneck for the planner is posture generation. Several improvements can be made on this point to speed up the computations. First, the sons generation is highly parallelizable: in the Contact VBFP presented here, all the posture generation problems are independent when expanding a node. We will soon make the necessary code changes to benefit from the multi-core feature of modern computers. It is also necessary to reconsider the solver with respect to our problem. By taking into account the particularities of the problem we solve, it is possible to write a dedicated solver and tune it carefully to converge rapidly toward an existing solution or detect quickly when there is no solution. We expect a huge speed gain from both the parallelization and the solver specialization.

Our planner generates contact sets; the motion transition between these sets is computed in a second step which is not considered here, but we started investigating two possible approaches: the first one uses semi-infinite optimization techniques [START_REF] Miossec | Development of a software for motion optimization of robots-application to the kick motion of the HRP-2 robot[END_REF][START_REF] Lengagne | Generation of dynamic motions under continuous constraints: Efficient computation using B-Splines and Taylor polynomials[END_REF], the other one is a closed-loop control approach using multiobjective optimization [START_REF] Liu | Interactive dynamics and balance of a virtual characters during manipulation tasks[END_REF][START_REF] Bouyarmane | Using a multi-objective controller to synthesize simulated humanoid robot motion with changing contact configurations[END_REF].

Fig. 2 .

 2 Fig. 2. Left: the foliations of the single support submanifolds. Right: an example of path (from bottom leaf to top one) with the following contact sequence:{F 1 } , {F 1 , F 2 } , {F 2 } , {F 1 , F 2 } , {F 1 } , {F 1 , F 2 } , {F 2 } , {F 2 , F 3 } , {F 3 }, {F 1 , F 3 } , {F 1 }.The movements in double support phases cannot be represented due to the lack of dimensions of the 3d

Fig. 3 .

 3 Fig. 3. The different constraints defining Q.

Fig. 4 .

 4 Fig. 4. The architecture of the posture generator, as a dialog between an optimization algorithm (in green) which asks for the values or derivatives at a point of the problem functions and a simulator (in blue) which computes them. (For interpretation of the references to colour in this figure legend, the reader is referred to the web version of this article.)

Fig. 5 .

 5 Fig. 5. The search volume V is build as the product of the surface's parameters domain and the segment [0, 2π ] (left). When a new contact with parameters P is found, a volume V P is constructed around (middle) and V \ V P is divided into convex sub-volumes (right).

Fig. 6 .

 6 Fig. 6. Starting from a piecewise linear path (dotted line, on the left) we construct a smoother path (plain line), upon which we base a descending valley-like shaped potential field (right).

Fig. 7 .

 7 Fig. 7. VBFP (black) versus BFP (black and gray). L is the list of leaves in BFP, L ′ in VBFP. Starting from node A, only the best son (C ) is generated at first with VBFP whereas BFP would generate all five sons (left). Then (center), C is chosen to be expanded in which case its best sibling (E) replaces it in L ′ , and the best son of C is generated. Finally (right), E being the best leaf in L ′ (or L), it is expanded by only its best sons J, and replaced by its best sibling (B) in L ′ . The first element of L ′ is always the same as the first one of L, ensuring the correctness of VBFP.

Fig. 8 .

 8 Fig.8. Initial, final and some in between steps for the three scenarios.

Fig. 9 .

 9 Fig. 9. Key postures to define the rough path p for the table scenario.

  N, the set of the nodes already generated during the whole planning Sons generation For a node n = (f , s, q), the generation of sons is based on the 1-neighbors of s, i.e. the sets of contacts obtained from s by either adding or removing a contact. The function generateSons presented in Algorithm 3 is therefore divided in two parts. The first part explores the possibility to remove a contact from s. It trivially tries every contact, one by one. A new node candidate n ′ is create at line 8, using the witness posture of its father for the initialization. Data: n, a node, i and j, indices of the contact spot and the surface Result: l, a list of leaves whose father is n, initially empty 1 -N is the set of the nodes already generated

	3 begin 4 (f , s, q) ← n // Suppressing a contact 5 if length(s) ≥ 2 then 6 for each c in s do 7 s ′ ← s\ {c} 8 n ′ ← (n, s ′ , q) 9 if add(n ′ ,N) then 10 if generate(n',c) then 11 l ← l ∪  n ′  // Adding a contact E surfaces  do 15 l ← l ∪ generateNewContacts(n,i,j) 16 return(l) Algorithm 3: 2 begin 3 if forbiddenContact(i,j) then 4 return(⊘) 5 (f , s, q) ← n 6 contacts ← getContactCandidates(i,j) 7 for each c in contacts do 8 s ′ ← s ∪ {c} 9 n ′ ← (n, s ′ , q) 10 if add(n ′ ,N) then 11 if generate(n',c) then 12 l ← l ∪  n ′  13 return(l) Algorithm 4: New contacts for a given couple (contact spot, surface)

1 -s, a list of contacts, initially empty 2 -12 for i ← 1 to size(E contacts ) do 13 if no isAlreadyInContact(s, i) then 14 for j ← 1 to size  The block on lines 9-11, which also appears in Algorithm 4, checks the validity of this candidate with respect to two criteria. The first test is the transposition of the loop avoidance: add(n, N) checks if n appears in N. If not, it adds it to N and returns true, otherwise it does nothing and returns false. It is implemented by defining a strict order relation ≺ on S (see 5.3). This way N is represented as a red-black binary tree and the existence test/insertion can be performed in O(ln(size(N))).

Algorithm: generateNewContacts

  Denoting by N O the number of objects in the environment, I coll is a subset of [|1, n+1|]×[|1, N O |]: pairs (r i , O k )

)(b) and (c) express the auto-collision and collision avoidance constraints, d(O 1 , O 2 ) being the (signed) distance between objects A and B. As in Section 4, r i (q) (resp. O k ) denotes the volume of the workspace occupied by the i-th body r i of the robot at configuration q (resp. k-th obstacle). ϵ ij and ϵ ik are security distances, thus usually positive. I auto is a subset of [|1, n + 1|] 2 : some pairs of bodies can never collide because of joint limits or of other collisions that will always occur before, and it is not always possible to write a collision avoidance constraint as a positive distance requirement.


F Lecellier

S Jehan-Besson

J Fadili

G Aubert

M Revenu

E Saloux

REGION-BASED ACTIVE CONTOUR WITH NOISE AND SHAPE PRIORS

Keywords:

In this paper, we propose to combine formally noise and shape priors in region-based active contours. On the one hand, we use the general framework of exponential family as a prior model for noise. On the other hand, translation and scale invariant Legendre moments are considered to incorporate the shape prior (e.g. fidelity to a reference shape). The combination of the two prior terms in the active contour functional yields the final evolution equation whose evolution speed is rigorously derived using shape derivative tools. Experimental results on both synthetic images and real life cardiac echography data clearly demonstrate the robustness to initialization and noise, flexibility and large potential applicability of our segmentation algorithm.

INTRODUCTION

The current work is devoted to the segmentation of regions of a priori known shape in noisy images using region-based active contours [START_REF] Cremers | Diffusion snakes : Introducing statistical shape knowledge into the Mumford-Shah functional[END_REF][START_REF] Tsai | A shape-based approach to the segmentation of medical imagery using level sets[END_REF][START_REF] Gastaud | Tracking video objects using active contours and geometric priors[END_REF]. This method allows the use of photometric image properties, such as texture and noise, as well as geometric properties such as the shape of the object to be segmented. The shape prior can prove very useful in cases where the object is occluded or partially missing. Furthermore, by including an a priori on the shape, sensitivity of the active contour model to initialization will also be alleviated.

On the one hand, attempts to incorporate shape priors have been proposed by some authors using various methods, such as diffusion snakes [START_REF] Cremers | Diffusion snakes : Introducing statistical shape knowledge into the Mumford-Shah functional[END_REF] or distance function [START_REF] Tsai | A shape-based approach to the segmentation of medical imagery using level sets[END_REF][START_REF] Gastaud | Tracking video objects using active contours and geometric priors[END_REF][START_REF] Leventon | Statistical Models for Medical Image Analysis[END_REF]. On the other hand, there are only few proposals in the literature that tried to take benefit of a noise prior [START_REF] Martin | Influence of the noise model on level set active contour segmentation[END_REF][START_REF] Lecellier | Statistical region-based active contours with exponential family observations[END_REF][START_REF] Galland | Multi-component image segmentation in homogeneous regions based on description length minimization: Application to speckle, Poisson and Bernoulli noise[END_REF] within region-based active contours. In these works, image features (e.g. intensity) are considered as random variables whose distribution belongs to some parametric family which is chosen according to the physical acquisition model of the considered images. However, to the best of our knowledge, shape and noise priors have never been combined, at least formally, in active contour models. This would enable to perform the segmentation of poor noisy images, beyond the simple classical white Gaussian noise model, with a strong shape constraint. Example of such data is encountered in echocardiographic images. For instance, in echographic data, it is notably well known that under appropriate conditions (large number of randomly located scatters), the Rayleigh distribution is well suited to model the noise [START_REF] Goodman | Some fundamental properties of speckle[END_REF].

The main contribution of this paper is to combine formally noise and shape priors in region-based active contours for segmentation purposes. In order to fix ideas, let us consider a region of interest Ω in the image. We propose to find the partition of the image that minimizes the following generic criterion which is able to handle both noise and shape priors:

J(Ω) = Ω f n (x, Ω)dx + α d(Ω, Ω ref )
where Ω ref represents the reference region shape, and x = [x, y] T stands for the location of the pixel.

The first term corresponds to the noise prior term. This term takes benefit of statistical properties of the image intensity. It is based on functions of parametric probability density functions (pdf) belonging to the exponential family. Probability models with these common features include Normal, Bernoulli, Binomial, Poisson, Gamma, Beta, Rayleigh, etc. These models are the most commonly encountered in imaging acquisition systems. This term is detailed in Section 3. The second term d(Ω, Ω ref ) corresponds to the shape prior. Shapes are here described using scale and translation invariant Legendre moments as in [START_REF] Foulonneau | Geometric shape priors for region-based active contours[END_REF]. With such a shape descriptor, the registration step is avoided. This term is discussed in Section 4.

The evolution equation of the deformable curve is deduced from the functional to minimize using shape derivative tools [START_REF] Delfour | Shape and geometries[END_REF] and the framework set in [START_REF] Jehan-Besson | Dream2s: Deformable regions driven by an eulerian accurate minimization method for image and video segmentation[END_REF][START_REF] Aubert | Image segmentation using active conturs : Calculus of variations or shape gradients[END_REF]. This paper is organized as follows: we briefly remind the shape derivation tools in Section 2. The noise model term is presented in Section 3. In Section 4, we introduce the shape prior model and the invariances that were added. The segmentation algorithm is presented in Section 5. Experimental results are discussed in Section 6. We finally conclude and give some perspectives.

SHAPE DERIVATIVE TOOLS

In order to be comprehensive, we here give a brief summary of the shape derivation theory. The interested reader may refer to [START_REF] Delfour | Shape and geometries[END_REF][START_REF] Jehan-Besson | Dream2s: Deformable regions driven by an eulerian accurate minimization method for image and video segmentation[END_REF][START_REF] Aubert | Image segmentation using active conturs : Calculus of variations or shape gradients[END_REF] for further details.

Let U be a class of domains (open, regular bounded sets, i.e. C 2 ) of R n , and Ω an element of U. The boundary ∂Ω of Ω is sometimes denoted by Γ.

The region-based term is expressed as a domain integral of a function f named descriptor of the region :

J r (Ω) = Ω f (x, Ω)dx (1)
In the general case, this descriptor may depend on the domain such as the descriptors introduced thereafter for noise and shape priors. The derivation of this term is performed using domain derivation tools. We apply a fundamental theorem [START_REF] Delfour | Shape and geometries[END_REF] which establishes a relation between the Eulerian derivative of J r (Ω) in the direction V, and the domain derivative of f denoted f s (x, Ω, V):

< J r (Ω), V > = Ω f s (x, Ω, V)dx - ∂Ω f (x, Ω) (V • N)da(x) (2)
where N is the unit inward normal to ∂Ω, da its area element.

The first integral comes from the dependence of the descriptor f (x, Ω) upon the region while the second term comes from the evolution of the region itself.

From the shape derivative, we can deduce the evolution equation that will drive the active contour towards a minimum of the criterion.

Let us suppose that the shape derivative of the region Ω may be written as follows:

< J r (Ω), V >= - ∂Ω v(x, Ω)(V(x) • N(x))da(x) (3)
We can then deduce the following evolution equation:

∂Γ(p, τ ) ∂τ = v(x, Ω) N(x)
with Γ(τ = 0) = Γ 0 , x = Γ(p, τ ).

THE NOISE MODEL

In this section we focus our attention on the noise model. The chosen descriptor for this part is:

f n (x, Ω) = Φ(p(y(x), η) (4)
where p is the pdf of some image features y(x) ∈ R d whose associated parameters are denoted by η, and Φ is at least C 1 .

In our study, we consider that p belongs to the exponential family. This family is comprehensive enough to cover noise models in most image acquisition systems encountered in practice, e.g. Gaussian, Exponential, Poisson, Rayleigh to cite a few. The multi-parameter exponential families are naturally indexed by a k-dimensional real natural parameter vector η = (η 1 , . . . , η k ) T and a k-dimensional natural sufficient statistic vector T = (T 1 , . . . , T k ) T . A simple example is the normal family when both the location and the scale parameters are unknown (k = 2). Formally, the pdf of a vector of random variables Y belonging to the k-parameter canonical exponential family is:

p(y, η) = h(y) exp[ η, T(y) -A(η)] (5)
where η, T denotes the scalar product. This statistical criterion is now derived according to the domain in order to deduce the evolution equation of the active contour. For the sake of simplicity, we denote η for the natural parameter of a pdf of the exponential family and its finite sample estimate over the domain (without a slight abuse of notation, this should be η).

Theorem [START_REF] Cremers | Diffusion snakes : Introducing statistical shape knowledge into the Mumford-Shah functional[END_REF] The Gâteaux derivative, in the direction of V, of the functional J n (Ω) = Ω Φ(p(y(x), η(Ω)))da(x) where p(.) belongs to the multi-parameter exponential family with natural hyperparameter vector η, is:

< J n (Ω), V >= - ∂Ω Φ(p(y))(V • N)da(x) + Ω p(y)Φ (p(y)) ∇ V η, T(y) -∇A(η) dx ( 6
)
with ∇ V η the Gâteaux derivative of η in the direction of V, and x, y the scalar product of vectors x and y.

In a finite sample setting, when using the ML estimator, we can replace ∇A(η) by T(Y) (the 1st order sample moment of T(Y)). Thus, when using the -log-likelihood function, the second term becomes equal to Ω ∇ V η, T(y) -T(Y)) dx, and hence vanishes. The following corollary follows:

Corollary [START_REF] Cremers | Diffusion snakes : Introducing statistical shape knowledge into the Mumford-Shah functional[END_REF] The Gâteaux derivative, in the direction of V, of the functional J n (Ω) = -Ω log(p(y(x), η ML (Ω))da(x) when ηML is the ML estimate, is the following:

< J n (Ω), V >= ∂Ω (log (p(y(x), η ML (Ω)))(V • N)da(x)
These general results can be easily specialized to some pdf of interest (e.g. Gaussian, Rayleigh, etc). We let the reader refer to [START_REF] Lecellier | Statistical region-based active contours with exponential family observations[END_REF] for more details.

THE SHAPE PRIOR MODEL

The shape prior is used as an additional fidelity term (e.g. to a reference shape), designed to make the behaviour of the segmentation algorithm more robust to occlusion and missing data and to alleviate initialization issues. Here, orthogonal Legendre moments with scale and translation invariance were used as shape descriptors [START_REF] Foulonneau | Geometric shape priors for region-based active contours[END_REF]. Indeed, moments [START_REF] The | On image analysis by the methods of moments[END_REF] give a region-based compact representation of shapes through the projection of their characteristic functions on an orthogonal basis such as Legendre polynomials.

The shape prior is then defined as the Euclidean distance between the moments of the evolving region and ones of the reference shape,

d(Ω, Ω ref ) = λ(Ω) -λ(Ω ref ) 2 2 (7)
where λ(Ω) are the moments of the region Ω. In practice, infinite moment expansion is generally limited to a sufficient finite number resulting in a good approximation of the original shape. The criterion then reduces to:

d(Ω, Ω ref ) = p+q≤N p,q (λ pq (Ω) -λ pq (Ω ref )) 2 (8)
where the λ pq are defined as follows, using the geometric moments M pq and the coefficients a pq of the Legendre polynomials [START_REF] The | On image analysis by the methods of moments[END_REF]:

λ pq = C pq p u=0 q v=0
a pu a qv M uv [START_REF] Foulonneau | Geometric shape priors for region-based active contours[END_REF] where

C pq = (2p+1)(2q+1) 4
, M pq (Ω) = Ω x p y q dxdy, and the Legendre polynomials are defined as :

P p (x) = p k=0 a pk x k = 1 2 p p! d p dx p (x 2 -1) p .
In general, the reference shape can have different orientation and size compared to the shape to be segmented. This will then necessitate an explicit registration step in order to realign the two shapes. In order to avoid this generally problematic registration step, we here use scale and translation invariant Legendre moments as in [START_REF] Foulonneau | Geometric shape priors for region-based active contours[END_REF]. In the geometric moments definition, the scale invariance is embodied as a normalization term:

1 Ω (p+q+2)/2 . As far as translation invariance is concerned, we replace x and y in the geometric moments M pq by x -x and y -ȳ, (x, ȳ) are the shape barycenter coordinates.

The derivation of the criterion ( 8) is relatively complex. We here give the main formula.

< d (Ω, Ω ref ), V >= u+v≤N u,v A uv (H uv + L uv ) N where Auv = 2 p+q≤N X p,q (λpq-λ ref pq )Cpqapuaqv Huv = (x -x) u (y -ȳ) v Ω (u+v+2)/2 Luv = uxMu-1,v Ω 3/2 (1-x)+ v ȳMu,v-1 Ω 3/2 (1-y)- (u + v + 2)Mu,v 2Ω
The reader may refer to [START_REF] Foulonneau | Geometric shape priors for region-based active contours[END_REF] for further details.

SEGMENTATION WITH JOINT NOISE AND SHAPE PRIORS

The region-based active contour functional to be minimized is finally written as:

J(Ωin, Ωout) = Z Ω in fn(x, Ωin) dx + α d(Ωin, Ω ref ) + Z Ω out fn(x, Ωout) dx + β E b (Γ) (10)
where we assign a specific noise model to the background (outside) region, possibly different from the noise model of the object (inside) region. The energy term E b is a regularization term balanced with a positive real parameter β. It can be chosen as the curve length and classically derived using calculus of variation or shape derivation tools.

To drive this functional towards its minimum, the geometrical PDE corresponding to [START_REF] Delfour | Shape and geometries[END_REF] is iteratively run without the shape prior, then the shape prior term is updated, and the active contour evolves again by running the PDE with the shape prior. This procedure is repeated until convergence. This iterative optimization scheme has a flavour of coordinate relaxation. At this stage, it is worth pointing out some major differences between our algorithm and the one developed in [START_REF] Foulonneau | Geometric shape priors for region-based active contours[END_REF]. The first one is that we here consider both photometric (noise) and geometrical (shape) priors, while [START_REF] Foulonneau | Geometric shape priors for region-based active contours[END_REF] focused on the shape prior and did not considered noisy data. This difference has a clear impact on the evolution algorithm since those authors propose to run the evolution equation only once without the shape prior and then incorporate the shape prior in the evolution. This is fundamentally different from our alternating scheme. Evolution using shape prior for 1 iteration.

Algorithm 1 Evolution algorithm of the active contour

5:

until Maximum shape speed < threshold 6: until Convergence

EXPERIMENTAL RESULTS

The above evolution scheme was applied on some synthetic data with y(x) = I(x) the image intensity. Fig. 1.(a) depicts a shape corrupted by an additive white Gaussian noise with SNR=1, with the initial curve. To bring to the fore the contribution of the shape prior term, parts of the objects are deliberately missing. Fig. 1.(b) (resp. (c)) shows the segmentation result with the noise model (Gaussian) but without (resp. with) the shape prior. As expected, one can clearly see that: (i) without a shape prior, the final curve sticks to the apparent boundaries of object, (ii) owing to the shape prior, the algorithm managed to recover properly the missing parts of the object. Furthermore, in addition to its robustness to missing data, we have also observed that the shape prior allows to mitigate initialization issues. As far as the noise prior is concerned, choosing the appropriate model has a clear impact on the quality of the results as it has been shown in [START_REF] Lecellier | Statistical region-based active contours with exponential family observations[END_REF].

We then tested our approach on real echocardiographic images. As the Rayleigh distribution is well suited to model the noise in these data [START_REF] Goodman | Some fundamental properties of speckle[END_REF], this noise model was used in Corollary 1. The original image (Fig. 2 tial contour position. We compared the result of our method (fig. 2), with (d) and without (c) the shape prior, to an expert manual segmentation (b), and a segmentation provided by the Active Appearance and Motion Model (AAMM) method (e) designed for echocardiography [START_REF] Stegmann | Active appearance models: Theory and cases[END_REF][START_REF] Lebossé | Segmentation du contour de l'endocarde sur des séquences d'images d'échographie cardiaque[END_REF]. Again, the saliency of our method is obvious. Our method gives the closest segmentation to the expert manual delineation. This is quantitavely by the Hamming distance plots (f), showing that our method outperformes AAMM.

CONCLUSION AND PERSPECTIVES

This paper concerns the incorporation of both noise and shape priors in region-based active contours. The evolution of the active contour is derived from a global criterion that combines statistical image properties and geometrical information. Statistical image properties take benefit of a prespecified noise model defined using parametric pdfs belonging to the exponential family. The geometrical information consists in mini-mizing the distance between Legendre moments of the shape and those of a reference. The Legendre moments are designed to be scale and translation invariant in order to avoid the registration step. The combination of these terms gives accurate results on both synthetic noisy images and real echocardiographic data. Our ongoing research is now directed towards the integration of a complete shape learning step.

Fig. 1 .Fig. 2 .

 12 Fig. 1. a. Noisy image with initial contour, b. Final contour without shape prior, c. Final contour with shape prior.