Mehdi Benallegue
email: mehdi.benallegue@college-de-france.fr

Jean-Paul Laumond

Alain Berthoz
email: alain.berthoz@college-de-france.fr

Contribution of Actuated Head and Trunk to Passive Walkers Stabilization

Passivity-based walkers represent a model for human walking and a solution for low-energy locomotion for humanoid robots. The presence of an upper-body and even a head in this kind of systems is necessary as a better model for humans and to improve their usability. The benefits of these additions have never been studied, and no work experimented the addition of a head limb to a passivity-based walker. So, we aim, in this paper, to study the effects of the addition of these modifications on walkers with fully passive lower limbs. By comparing three systems (a passive compass, an upper-body stabilizing walker and a head stabilizing walker) simulations show that: (ii) upper-body stabilization improves the stability of the walking limit cycle; (ii) in return, the stabilization of the upper body requires a noticeable amount of the kinetic energy of the walker, and a significant energy supply (steeper slopes for the passive case) is necessary to guarantee the stability of the gait, especially for the case of head stabilization; and (iii) in a dynamical context, such as steep slopes, the upper-body and head stabilization have close performances for absorbing perturbations and smoothing the impacts, but with a slight advantage for the latter.

I. INTRODUCTION

Legged, and especially biped, locomotion is acknowledged to be the most adapted moving technique when sharing the same space as humans. However, there lies an intrinsic falling hazard in the dynamics a walker. A robot's state is said to be viable if the robot is able to avoid the fall [START_REF] Wieber | Viability and Predictive Control for Safe Locomotion[END_REF]. Walking is then a very dynamic task which requires energy. However, an important parameter for biped robot is the energy-efficiency of walking, and the robots that show the best efficiency are passive walking robots. Indeed, on shallow slopes, very simple mechanical designs with appropriate mass distributions show balanced and durable gaits with no energy source other than the gravity. This efficiency property was then adapted to generate low power walking systems on level ground by the use of simple actuators to compensate gravity [START_REF] Collins | Efficient bipedal robots based on passive-dynamic walkers[END_REF]. Moreover, passive walkers are commonly assumed to generate more human-like motions, even with the presence of geometrical or mechanical discrepancies between the robot and humans [START_REF] Collins | A three-dimensional passivedynamic walking robot with two legs and knees[END_REF]. This similarity has attracted the interest of biomechanics to study the passivity of the human gait [START_REF] Goswami | Limit Cycles in a Passive Compass Gait Biped and Passivity-Mimicking Control Laws[END_REF].

In order to improve the accuracy of this model for humans, the presence of a vertical upper-body is crucial, because the dynamics and mass contributions of the human's upper-body are important. Moreover, any biped robot that is intended to have actual usability other than a simple locomotion requires to have an upper body, for example to enable manipulation and interaction. Indeed, some researches have studied the addition of an upper-body to a passive walker, but none of them, to our knowledge, studied the effect of a vertical stabilization of this upper-body on the stability of the limit-cycle, nor the benefits on the set of viable states.

One other important feature of human walking, that does not enter into account within the aforementioned researches, is the role of the head and gaze in human locomotion. Indeed, it has been shown that during walking motions, the head is stabilized in rotation [START_REF] Hicheur | Head motion in humans alternating between straight and curved walking path: Combination of stabilizing and anticipatory orienting mechanisms[END_REF]. This stabilization is a general phenomenon observed for several human tasks and is believed to enable a more stable reference frame for egocentric visual motion perception and a more consistent visual-vestibular sensory inputs. In the same spirit, it has been shown that the head stabilization provides a major improvement of the estimation of the vertical direction for robots, particularly when relying on a vestibular-like inertial measurement system [START_REF] Farkhatdinov | Modeling verticality estimation during locomotion[END_REF]. An accurate estimation of the vertical is considered as a crucial parameter to ensure balance for humans and humanoid robots. However, no research, to our best knowledge, studied the possibility to add a head link to a passive walker.

We propose to study, in a pure mechanical point-of-view, the benefits of upper-body and head stabilization on the simplest human-like walking systems: a purely passive lowerbody. The paper aims particularly to study the case of the appearance of unexpected perturbations during cyclic walking. We propose then to compare the simulated dynamics of three models: (i) the classical compass walker (ii) a walker with a stabilized simple upper-body, and (iii) a novel passive walker model with an upper body and a head, both stabilized.

In the following section, we describe these three models, their control and our implementation. In the section III, we describe the results we obtained when evaluating the viability kernels of these walkers for different slope inclinations. And in the section IV, we show an in-depth analysis of the cyclic walking on shallow slope.

II. WALKING SYSTEMS

A. The mechanical model

We study in this paper two main 2D models for passive walkers: one with only two links (compass) and the other one equipped with upper-body segments, as depicted in Fig 1 . The walker has two massless legs of l p length, with, on each leg, a point mass of m l at l l distance to the joint between them. The trunk is a masseless stick of l t length, linked to legs, and having a point mass of m t on its middle. The head is a point mass of m h on the top of a stick of l h length, linked to the torso. To keep the simplicity of the model, we didn't study the case of the presence of knees, ankles or arms even if they have noticeable effects on the dynamics of the walker [START_REF] Kaddar | Arms swing effects on a walking planar biped[END_REF]- [START_REF] Chevallereau | Asymptotically stable walking of a five-link underactuated 3-d bipedal robot[END_REF]. There are two major ways to add an upper body to a passive walker. First, the bisector constrained walker, introduced by Wisse et al [START_REF] Wisse | Adding an upper body to passive dynamic walking robots by means of a bisecting hip mechanism[END_REF], is a compass with an upper limb that is constrained to be in the midway angle of the two legs. However, beside the fact that it is not an accurate model of human walking, its bisecting constraint introduces an instability, especially in the presence of a big upper body mass [START_REF] Asano | Pseudo virtual passive dynamic walking and effect of upper body as counterweight[END_REF], which is the case of humans. Therefore, none of our simulations succeeded to show any viable state for this model. The second method is to stabilize actively the upper body against the vertical. This method better models the human gait. The stabilization is achieved commonly by applying torque on the stance leg [START_REF] Mcgeer | Stability and control of two-dimensional biped walking[END_REF] which has the advantage to not disturb the passivity of the swing motion. The torque applied on the stance leg is denoted τ t . The head stabilization is ensured by another pure torque generator in the neck link, between the head and the trunk. This torque is denoted τ h .

The walker has always a stance leg on the ground. That makes the compass position fully describable with four angles: the inter-legs angle (θ) and the stance leg, torso and head inclinations with regard to vertical (φ, α and β respectively).

We study also the case where there is no upper body, which is equivalent in our case to consider l t = l h = 0. In that case the angles α and β have no influence on the dynamics of the compass and can be ignored.

B. The dynamics state

The dynamics state x of the walker can be defined by the limbs angles and their angular velocities x = θ φ α β θ φ α β t . The evolution of the state over time is continuous except in impacts. The impacts on the ground are considered perfectly inelastic and stiff. That introduces discontinuities in the angular velocities, and the stance leg switch introduces a change in the sign of θ. Therefore, we prefer a discreet time expression of the evolution of the dynamics state. We consider the future discretized state x k+1 as completely determined by the previous state x k and the torques vector τ k = τ t,k τ h,k t :

x k+1 = f (x k , τ k ) (1)
The function f integrates the dynamics equations over sampling time. These equations won't be detailed here and can be adapted from former works [START_REF] Goswami | Compass-Like Biped Robot Part I : Stability and Bifurcation of Passive Gaits[END_REF]. We used a software dynamic simulator to compute the dynamics and detect contacts during walking motions. We consider that contacts always happen on a sampling instant, and that the dynamics state during this instant is the pre-contact one. The contacts are considered without slipping or deformation. The falling state is detected in three cases: when (i) there is no contact on the ground, (ii) there is a contact with a limb other than a foot, or (iii) a double support lasts for more than one instant, which corresponds to a stop in the walking motion.

C. The controllers

The control has to be different from a walker model to another. For example for the simple two-links walker, the torques have no effect on the dynamics and are ignored. The walkers equipped with an upper body have to be actively controlled to maintain the trunk and the head above the ground.

1) Trunk stabilization controller: To control the robot with one-link upper-body, we add a rigid constraint on the neck joints to guarantee that α = β. The proposed controller is a proportional derivative (PD) servoing the upper-body to be vertical, i.e. α = 0. The torque expression is then:

τ 1,t = -K 1,p α -K 1,d α (2)
With K 1,p and K 1,d are the proportional and derivative gains of the controller. The controller behaves like an angular spring-damper on the trunk-to-stance joint. However, we cannot assume any actually realizable equivalent setting with a passive spring-damper because the vertical reference is defined in absolute frame and the stance leg changes at each step.

2) Head stabilization controller: The controller we suggest for the neck is also a PD bringing back β to zero:

τ 2,h = -K 2,p β -K 2,d β (3)
This walker requires also to control all the upper-body and maintain it upright. The controller we propose for trunk stabilization will try maintain the center of mass of the upper body c u above the hips. To do so, we use another PD to control the inclination µ of the line joining c u to the hips (see Fig. 2-C):

τ 2,t = -K 3,p µ -K 3,d μ (4) B A C

D. The simulated walkers parameter values

For the robots equipped with an upper-body, we have chosen to use a human-like mass distribution and body proportions [START_REF] Armstrong | Anthropometry and mass distribution for human analogues. volume 1. military male aviators[END_REF], but by simplifying and rounding them for simulation reproducibility. Even if the two-links compass doesn't have an upper body, we kept the same masses distribution to guarantee a coherent comparison. The masses are: m l = 1.5kg, m t = 4.5kg, m h = 0.5kg and body lengths are l p = 1m, l l = 0.6m, and for upper-body equipped walkers: l t = 0.75m and l h = 0.125m.

The controllers parameters were also fixed. For the trunk stabilization we set K 1,p = 30N•m/rad and K 1,d = 15N•m•s/rad. We limited the absolute value of the torque |τ t | < 50N•m. This value is not bio-inspired but only prevents the system from producing excessive torques. For the head stabilization, we choose to have the same upper body stabilization parameter, the controller parameters are then also

K 3,p = 30N•m/rad, K 3,d = 15N•m•s/rad, with |τ t | < 50N•m. For head stabilization, we took K 2,p = 5N•m/rad and K 2,d = 0.5N•m•s/rad. The neck torque τ h had two limitations: |τ h | < 10N•m
during the double support instant in order to absorb the impact, and |τ h | < 2N•m otherwise. These values permit to enable the best impact absorption in this joint while keeping the torque reasonable during the rest of the time.

With these models, we can finally start the simulations.

III. VIABILITY KERNEL VS ENERGY CONSUMPTION

We aim in this first study to compare the robustness of the three walkers to a variety of initial conditions on different slope inclinations. Then, we define a relaxed definition of a viable state which is: a state from which the walker is able to perform at least 6 steps. Although this new definition does not guarantee the balance of the walker for all future steps, it is a good indicator of the viability kernel and has a fast enough computation to permit to check large number of states.

To compare the performances of these walkers, they have to start from the same initial state. However, the state vector itself is different from a walker to another. Indeed, the twolinks compass has a state vector with only relevant variables, θ φ θ φ t . For the stabilized-trunk walker, there are also α and α. And for the stabilized-head walker there are β and β also. Therefore, for the last two ones, we provide constant initial values for α, α, β and β. For each walker, the initial state is its mean value during double support, after it reaches its limit-cycle on 4˚slope (this limit-cycle will be detailed in subsection IV-B). Specifically, for the trunk stabilized walker the values are α 0 = β 0 = 0.0454rad α0 = β0 = 0.251rad/s. For the head stabilized walker, the values are α 0 = 0.0643rad, β 0 = 0.0028rad, α0 = 0.289rad/s, and β0 = 0.176rad/s. After this initialization, they have the natural dynamics during all the simulation. The choice of these parameters is coherent with our idea to study the effects of unexpected perturbations during cyclic walking: the walkers are supposed to be stable in their cycle before the beginning of the simulation.

All the starting states, in our simulations, are in double support phase, during the pre-impact instant. In that instant, the knowledge of the slope angle γ and the inter-leg angle θ is sufficient to give the stance leg inclination φ. The initial state is then simplified to three scalars: θ, θ and φ.

The simulations are run for five different slope inclinations: 2˚, 4˚, 6˚, 8˚and 10˚. For each inclination and each walker model, we ran simulations for 8 millions different states, which corresponds to 200 different values for each of the three dimensions. These values were uniformly distributed within ranges of values that cover all the viable states we could simulate. These ranges are [0, 1.309]rad for θ 0 , [-11.4, 130]rad/s for θ0 , and [0, 8]rad/s for φ0 .

For each initial state, the walker starts the simulation at double support state and the steps it makes are counted. If it reaches 6 steps, the initial state is said to be viable, if it falls before, the state is non viable. The sampling period is of 5ms and the simulations have a complete duration of 90 hours.

The most important result we compare in this simulation is the volume of the viability kernel by counting the number of viable states among the 8 million combinations. The results are shown in Fig. 3.

One striking property in this figure is the difference of results between shallow and more steep slopes. For less than 6˚slopes, the simple two legs compass has a bigger viability kernel than upper-body equipped ones, and for steeper ones the upper body gives better results than a simple compass. The analysis we provide for this result is that the upper body does stabilize the walking motion, but this stabilization absorbs the total kinematic energy of the walker. As the only external source of energy is gravity, shallow slopes do not provide enough energy to compensate the loss due to stabilization. The two legs compass is much more energy-efficient, because the only loss of energy happens during impacts. Impacts depend on the angle of incidence of the swing leg on the ground, and in shallow slopes, the steps are much smaller and impacts preserve more kinematic energy. On the contrary, in steeper slopes, the highly dynamic context provides kinematic energy on one hand, and an important instability on the other. Here can be demonstrated the stabilizing properties of upper-body control, especially on highly dynamic instability. We see then that the upper body stabilization absorbs an important part of this energy and helps to recover balance. Moreover, the head stabilization appears to be better again than the trunk only. The difference is tight, without surprise, because the mass contribution of the head is relatively less important. But we can say that there are states that are not viable for the trunk stabilization alone, and that can be recovered when the head is stabilized. The consequence of the control of the head is to distribute the stabilization of the upper body on more than a link. This result shows then that dividing the upper body on two (or more) hierarchically stabilized segments can improve the recovery for highly dynamical instability.

In this section, we have analyzed quantitatively the volume of viability kernel for the three walker models. We have seen that at 4˚slope, the two-links compass has a bigger viability kernel than the two other walker models. However, by analyzing deeper this specific result in the next section, we will see that the distribution of the viable states of a twolinks compass is not optimal regarding small perturbations to cyclic walking. We will see also that upper-body stabilization improves the stability of the walking limit cycle compared to a two-links compass.

IV. LIMIT CYCLE WALKING

A. Sensitivity to perturbations

The context we are analyzing is particularly the recovery from unexpected perturbations. In such situation, the state of the walker is probably close to the limit cycle. From this perspective, we projected the viability kernels along the three dimensions, and studied their position compared to the limit cycle state. The mean limit cycle states are 0.625 0.71 1.6 t for the compass, 0.56 -0.357 1.42 t for the trunk stabilization and 0.54 -0.23 1.41 t for the head stabilization.

The figures 4, 5 and 6 show the distribution of this 'error', where zero value means that the corresponding component of the state is the same as in the limit cycle. The densities have been normalized to have a total integral of 100%. We see in Fig. 4 and 6 that the kernels are almost equally close to zero: the three models have similar sensitivities to variations in the the inter-leg angle θ and stance inclination rate φ (see Table I for the mean values of this error along each of the dimensions). On the contrary, the Fig. 5, related to the inter-leg angle rate θ, shows an important difference between upper-body equipped walkers which are centered on the limit cycle and the simple compass which is much more asymmetric around zero, which is confirmed by the Table I. That means that the compass is more sensitive to perturbations in θ. In fact, if we reduce the state space to 15rad/s around the limit cycle, the number of viable states drops to 24447 for the compass, 23385 for the trunk stabilization and 21615 for the head stabilization. That makes them finally almost equivalent against perturbations on the limit cycle.

Despite the equivalence in the volumes of the viability kernels, this particular sensitivity to θ can make the limit cycle less regular for two-links compass. In order to study this sensitivity, let's analyze the cyclic walking itself, and specifically the stability of this cycle. I. We clearly see here that the mean position of θ in the the viability kernel of the compass is much farther from the limit cycle than for other walkers.

B. Stability of the cycle

In the perspective of studying the stability of the dynamical walking, we have chosen to analyze the trajectory of one of the legs of the walkers, let's say it's the left one. We simulate the walking motion, and analyze the trajectory of the inclination ψ of the left leg. In Fig. 7, 8 and 9, the phase plot of limit-cycle left leg inclination is displayed for each of the walker models, when walking during 75 seconds on a 4˚slope (see [START_REF] Goswami | Compass-Like Biped Robot Part I : Stability and Bifurcation of Passive Gaits[END_REF] to help interpreting these plots). These limit-cycles were obtained by simulating a walking motion during 2000 steps before to start the plot. The first observation is that the upper-body walkers have smaller amplitude, in left leg inclinations, than compass one, which means that they make smaller steps. This has already been analyzed as a direct effect of upperbody stabilization [START_REF] Hoshino | A passive walking strategy for a biped robot with a large mass torso by a spring and a damper[END_REF]. More interestingly, the trajectory for the compass presents a chaotic behavior [START_REF] Goswami | Compass-Like Biped Robot Part I : Stability and Bifurcation of Passive Gaits[END_REF]. This chaos means that the fixed point of the Poincaré map is unstable. We see some trajectories with very small pre-impact swing leg retraction and even impacts that create a discontinuity in the opposite direction to usual ones, which is a sign of an important instability during walking. The only part which is regular is the stance phase, which corresponds to the case where ψ = φ. This is coherent with the previous observation that the compass is less sensitive to the variations of φ and φ than to variations of θ. To analyze this sensitivity, there are dedicated methods such as the Floquet multipliers [START_REF] Hobbelen | Limit cycle walking[END_REF], but they are not adapted to our case because they are not well defined in the case of chaotic behavior. A phase plot of the evolution of left leg inclination of a two-links compass for 75 seconds on a 4˚slope. We see that the limit oscillation is chaotic.

For other walkers, we can see also a chaotic motion, but the amplitudes of the state values is noticeably smaller. we see regular-limit cycles for the left leg inclination. The plots are quite similar, but noticeable differences are visible in post impact trajectories, this shows that trunk stabilization and head stabilization don't deal with impacts in the same way. This the topic of the next and last analysis of this paper. We can see also that the walker maker smaller steps than the compass.

C. Energy and impacts

The energy loss in collision is an indicator of the severity of the impacts. The smoothest the impact is, the more efficient is the walking motion, in terms of dissipation of energy. For A phase plot of the evolution of one leg inclination of the stabilized head walker for 75 seconds on a 4˚slope. The cycle is similar to the stabilized trunk one, but we notice higher dynamics phases after the impacts.

the same period of 1000 steps, the compass had a mean energy loss of 3.20J by step, the trunk stabilization: 1.76J and the head stabilization: 1.49J. This is due to the fact that the compass has a CoM up to 26 cm lower and bigger steps. For illustration, in 1000 steps, the compass traveled for 593 meters, the trunk stabilization walker 536 meters and the head stabilization one made 533 meters. Moreover, there is a part which is absorbed by the actuator itself, the mean power was of -1.97W for the trunk stabilization, and -3.16W for the head stabilization. That means more kinematic energy is absorbed in head stabilization, which explains its 2˚slope disappointing performance in Fig. 3. Nevertheless, the advantage of this property is that impacts are much lighter, which permits better absorption of perturbations during highly dynamical instability, that explains the better performances for steep slopes in Fig. 3.

V. DISCUSSION AND CONCLUSION

We have seen throughout this study the effects, in mechanical ways, of the addition of an upper-body and a neck joint to a passive walking system. The upper-body stabilization regulates naturally the limit cycle of these walkers on shallow slopes and improves the balance and perturbation recovery for steeper slopes. However, these features consume the kinematic energy of the walker and require a minimal source of energy, this explains the bad performances of this model in nearly flat grounds.

Our results lead us to believe that adding actuation to the legs is required to overcome directly the energy supply issue. Doing so, upper-body stabilization is expected to contribute to generate stable motions in shallow slopes, and even on rough flat terrain. Furthermore, adding actuation to a passive walker can improve the stability of the walking motion. However, the control of passivity-based walkers is an active research topic that requires a subtle choice of the control strategy to use. This choice is usually a difficult compromise between power efficiency and balance enhancement. This will be the topic of our next study.

This work opens the way to other extensions, for example in terms of mechanical model. We can divide the upper-body into more segments to constitute a vertebral column. Our results suggest that a more complex upper-body can improve again the stability of walking motions.

MEDIA ATTACHMENT

The attached video shows (A) a simulation of the limitcycles for the three walkers on 4˚slope, with a speed-up to better notice irregularities, particularly in the two-links compass case. And (B) examples of slope inclination changes that the upper-body walkers overcome and not the compass.

Figure 1 .

 1 Figure 1. Our walker model. The mass on the trunk is at the middle of the trunk joint. The case of two segment compass corresponds to lt = l h = 0.

Figure 2 .

 2 Figure 2. The three walker models. A-The two-links compass has no upper body and no actuator. B-The walker with a simple-trunk stabilization has only one link on the upper body. C-The upper body of the walker with a stabilized head has two links. The controller brings back µ to zero, i.e. the center of mass of the upper body cu vertically on the hips.

Figure 3 .

 3 Figure 3. The volumes of viability kernels of the three models, for different slope inclinations (number of viable states among 8 million combinations).

Figure 4 .

 4 Figure 4. The marginal density, associated to the inter-leg angle θ, of the error between the limit cycle state and the viability kernel states, for the three models, on 4˚slope.

Figure 5 .

 5 Figure 5. The marginal density, associated to the inter-leg angle variation θ, of the error between the limit cycle state and the viability kernel, for the three models, on 4˚slope.

Figure 6 .

 6 Figure 6. The marginal density, associated to the stance leg inclination rate φ, of the error between the limit cycle state and the viability kernel, for the three models, on 4˚slope.

Figure 7 .

 7 Figure 7. A phase plot of the evolution of left leg inclination of a two-links compass for 75 seconds on a 4˚slope. We see that the limit oscillation is chaotic.

Figure 8 .

 8 Figure 8. A phase plot of the evolution of left leg inclination of the stabilized trunk walker for 75 seconds on a 4˚slope. The cycle is much more stable. We can see also that the walker maker smaller steps than the compass.

Figure 9 .

 9 Figure 9. A phase plot of the evolution of one leg inclination of the stabilized head walker for 75 seconds on a 4˚slope. The cycle is similar to the stabilized trunk one, but we notice higher dynamics phases after the impacts.

ACKNOWLEDGMENT

This work is partially supported by grants from the EU Project CLONS (Closed-Loop Neural Prosthesis for Vestibular Disorders, Grant Agreement 225929).


S Miossec

A Strictly Convex Hull for Computing Proximity Distances With Continuous Gradients

Adrien Escande, Sylvain Miossec, Mehdi Benallegue, and Abderrahmane Kheddar, Member, IEEE Abstract-We propose a new bounding volume that achieves a tunable strict convexity of a given convex hull. This geometric operator is named sphere-tori-patches bounding volume (STP-BV), which is the acronym for the bounding volume made of patches of spheres and tori. The strict convexity of STP-BV guarantees a unique pair of witness points and at least C 1 continuity of the distance function resulting from a proximity query with another convex shape. Subsequently, the gradient of the distance function is continuous. This is useful for integrating distance as a constraint in robotic motion planners or controllers using smooth optimization techniques. For the sake of completeness, we compare performance in smooth and nonsmooth optimization with examples of growing complexity when involving distance queries between pairs of convex shapes.

Index Terms-Bounding volume, continuous gradients of proximity distances, smooth and nonsmooth optimization, sphere-torus patches, strictly convex hulls.

I. INTRODUCTION

T HE distance function and its use in collision-free robotic planning are remarkably addressed by the seminal work of Gilbert and Johnson [START_REF] Gilbert | Distance functions and their application to robot path planning in the presence of obstacles[END_REF], which provides a thorough analysis of its mathematical properties. In [START_REF] Gilbert | Distance functions and their application to robot path planning in the presence of obstacles[END_REF], motion planning is formulated as an optimization problem in which collision avoidance is defined as inequality constraints. Indeed, each collision avoidance constraint is written in terms of the Euclidian distance between the robot and the obstacles: this distance must always be above a certain positive threshold.

A particularly important theorem in [START_REF] Gilbert | Distance functions and their application to robot path planning in the presence of obstacles[END_REF]-that we rediscovered and proved differently in [START_REF] Escande | Continuous gradient proximity distances for humanoids free-collision optimized-postures generation[END_REF]-is that the distance function for a pair of objects is not continuously differentiable unless one of the objects is convex and the other is strictly convex. As we illustrate later, nondifferentiability of the distance function can be a problem when used in smooth optimization (see theoretical explanations in [START_REF] Bonnans | Numerical optimization-Theoretical and Practical Aspects[END_REF]): the optimization solver might not converge in some configurations. Then, why not use nonsmooth optimization solvers? This is an option. However, with respect to smooth optimization routines, nonsmooth ones are less easily available, less complete, and slower. Furthermore, it is worth adding distance constraints in existing schemes that already rely on smooth optimization routines (robotic models are generally smooth), or directly on function derivatives.

To still use smooth methods when they are beneficial, the idea is to enforce the continuous differentiability of the distance function. This is the subject of this paper. In [START_REF] Gilbert | Distance functions and their application to robot path planning in the presence of obstacles[END_REF], the problem is formulated in two dimensions using a rectangle representing the shape of a 3-degree-of-freedom (DOF) robot moving among fixed two-dimensional (2-D) convex obstacles. To add continuously differentiable collision avoidance constraints between the convex shapes of the robot and the obstacle, Gilbert and Johnson suggested the idea of lightly "bulging" the rectangle approximating the shape of the robot so as to enforce the strict convexity of its shape. Thus far, the automation of the "bulging" and its extension to the 3-D case have still not been addressed: this is the core contribution of our study.

We propose a solution for the 3-D case [START_REF] Escande | Continuous gradient proximity distances for humanoids free-collision optimized-postures generation[END_REF]: an automated and robust geometrical operator to construct a strictly convex shape of a given geometrical model. In other words, we present a method to bulge (i.e., round or curve) a polyhedral convex hull into a strictly convex hull/shape. Besides, our operator provides tunable bulging, ranging from the sphere bounding volume (i.e., the sphere with minimum radius encapsulating the entire object) to a polyhedral convex hull (i.e., the convex polyhedron with minimum volume that bounds the entire object). Our operator builds like a polyhedral convex hull, except that vertices are patches from small spheres of predefined radius, faces are patches from predefined bigger-radius spheres, and edges are patches from tori connecting adjacent features; therefore, we named it sphere tori patches (STP), and since it is a bounding volume, we added BV, yielding the name STP-BV.

Our second contribution is a study of the impact of STP-BVs when used for distance computation in optimization problems. We take three examples of increasing complexity to study their usefulness in terms of convergence success, speed, and precision. We compare 1) two smooth solvers without STP-BVs distance regularization; 2) the same two solvers with STP-BV; and 3) a nonsmooth solver without STP-BV. The results show that the second option outperforms the others.

Many robotics problems require distance computation: motion planning, grasping, simulation, collision-avoidance, and design based on collision-free workspace computation. Smooth formulation of these problems could then benefit from the use of STP-BV. Examples of the use of STP-BVs can be found in [START_REF] Escande | Planning contact points for humanoid robots[END_REF] for planning, in [START_REF] Lengagne | Generation of whole-body optimal dynamic multi-contact motions[END_REF] for trajectory optimization, and in [START_REF] Khansari-Zadeh | A dynamical system approach to realtime obstacle avoidance[END_REF] and [START_REF] Stasse | Real-time (self)-collision avoidance task on a HRP-2 humanoid robot[END_REF] for control.

II. RELATED WORKS

There are many types of strictly convex shapes. Used as bounding volumes (covering at best the robot's links), they would guarantee the differentiability of the distance function. However, they either have a poor volume ratio or are timeconsuming when used in distance computations. Spheres, ellipsoids, and superellipsoids are such strictly convex shapes. A single sphere or ellipsoid per link exhibits a weak volume ratio fitting, particularly for relatively thin and long robot links. The use of parts of these shapes to better fit each link increases the number of constraints to be added in the optimization problem. By combining several spheres, k-IOS [START_REF] Zhang | k-IOS: Intersection of spheres for efficient proximity query[END_REF] can fit the object tighter, yet the ratio might be no better than 0.5 in pathological cases. Superellipsoids or even hyperquadrics are also very good candidates, but computing distance from these shapes to others is too time-consuming [START_REF] Chakraborty | Proximity queries between convex objects: An interior point approach for implicit surfaces[END_REF] to be considered in control. Furthermore, the volume ratio fitting for a general convex shape cannot be made arbitrarily close to 1.

Few other papers addressed the nondifferentiability of the distance between convex bodies. Rusaw in [START_REF] Rusaw | Sensor-based motion planning in SE(2) and SE(3) via nonsmooth analysis[END_REF] used nonsmooth analysis in the context of sensory-based planning. Patel et al. [START_REF] Patel | A collision-avoidance scheme for redundant manipulators: Theory and experiments[END_REF] used cylinders as BVs to cover a robot and acknowledged problems in the control when pairs of cylinders become parallel. They proposed solving this issue by thresholding the output speed of the robot. Recently, many authors acknowledged this problem when using capsules [START_REF] Dietrich | Integration of reactive, torque-based self-collision avoidance into a task hierarchy[END_REF] or convex hulls in optimization problems [START_REF] Lampariello | Trajectory planning for optimal robot catching in real-time[END_REF].

To avoid collisions, pseudodistances can also be used, provided they have good differentiability properties. Distance fields such as in [START_REF] Shapiro | Implicit functions with guaranteed differential properties[END_REF] are a possible solution. However, they remain computationally expensive as stated in [START_REF] Fuhrmann | Distance fields for rapid collision detection in physically based modeling[END_REF] and need to be adapted for a pair of moving objects. Closer to the Euclidean distance, Zhu et al. [START_REF] Zhu | A pseudodistance function and its application[END_REF] study the differentiability of a family of pseudodistances based on the gauge function of a convex polygon (thus encompassing the L 1 and L ∞ distances). These pseudodistances are differentiable almost everywhere in the same manner as the Euclidean distance. However, the sufficient condition for differentiability is not met in the same cases as those for the Euclidean distance as well as for additional cases (when a face of the Minkowski difference of the two objects is parallel to a face of the polygon defining the pseudodistance). Zhu et al. [START_REF] Zhu | A pseudodistance function and its application[END_REF] do not provide a way to avoid these cases.

III. MOTIVATION

A. Example of the Distance Gradient's Discontinuity in Two Dimensions

Let us consider the segment depicted in Fig. 1. d is the distance from this segment to the horizontal axis, and we look at the Fig. 3. Gradient ∂d/∂θ of the distance between the bar (left) or its bulged version (see Fig. 4) (right) and the horizontal axis. In the first case the gradient is discontinuous at θ = 0, while in the second case it varies quickly but continuously around θ = 0. (a) Gradient discontinuity (see Fig. 1). (b) Regularized gradient (see Fig. 4). Fig. 4. Segment is replaced by a part of circle with a big radius. Now, when going over θ = 0, the pair of witness points moves continuously. gradient of d with respect to θ. This gradient [see Fig. 3(a)] is shown to be discontinuous in [START_REF] Rusaw | Sensor-based motion planning in SE(2) and SE(3) via nonsmooth analysis[END_REF]. The discontinuity occurs when the segment becomes parallel to the horizontal axis: in this case, the witness point of the segment switches instantly between its two endpoints (see Fig. 2). This leads to a gradient discontinuity of d since both endpoints move in an opposite direction when θ changes. Such a point of discontinuity is named kink point.

B. Regularization of the gradient

To avoid this gradient discontinuity, the idea is to prevent jumps of the pair of witness points. This case occurs when flat parts of both objects' shapes become parallel. We remove the flat parts by bulging (i.e., curving) them. For instance, we replace the segment of our example by a part of a circle having a large radius (see Fig. 4).

The now unique witness point travels continuously between both endpoints and the gradient becomes continuous [see Fig. 3(b)]. This is the main idea behind the bounding volume that we describe in Section V. Subsequently, bulging an object is called gradient regularization or regularization for short.

C. Efficiency of the Regularization

Let us consider the following optimization problem:

min h,θ h 2 + (θ -θ b ) 2 (1)
s.t. d(h, θ) ≥ d 0 (2)
where h denotes the height of a 1-m bar center (l 1 = l 2 = 0.5 m in Fig. 1); θ, its orientation; d 0 > 0, a margin distance; and θ b , the target orientation. We can show that this optimization problem converges to the kink point (h, θ) = (d 0 , 0) for

-d 0 l 1 ≤ θ b ≤ d 0 l 2 .
In the other cases, the solution is a regular point. We compare several strategies to solve this problem (and the other ones discussed in this paper): using smooth optimization routines with or without regularization, and using nonsmooth routines without regularization. For the former, we use two solvers: FSQP [START_REF] Lawrence | A computationally efficient feasible sequential quadratic programming algorithm[END_REF] and NPSOL. 1 These are SQP algorithms, a class of routines among the fastest for nonlinear smooth problems, with a superlinear convergence. For the latter, we chose SolvOpt [START_REF] Kappel | An implementation of Shor's ralgorithm[END_REF]. Analytic center cutting plan methods, used, for example, by the optimization solver OBOE [START_REF] Nesterov | Homogeneous analytic center cutting plane methods for convex problems and variational inequalities[END_REF], deal with nonsmooth problems as well, but require the optimization problem to be convex, whereas a distance function provides a nonconvex constraint. Each solver has its own parameters for stopping criteria. Thus, we compare them by the number of iterations or computation time needed to obtain a desired precision of the solution (determined analytically when possible or by brute force). We consider two possibilities by choosing θ b appropriately: convergence to a regular point or to a kink point. Each case study is optimized 100 times with the same random initial conditions, and the results are quantified as mean values. Computations are performed for different regularization radii (when applicable) and different algorithm parameter values. For a fair comparison, the best results in terms of computation time with respect to (wrt) the solution precision are kept. This corresponds to having tuned the solvers to obtain the best of their capabilities for the problem. Results are presented in Fig. 5. Using Table I, one can also obtain the mean results in terms of the computation time. All the computation times given in this paper are obtained on an Intel Core 2 Duo P8600 (3M Cache, 2.40 GHz). Note the slower convergence to a kink point than to a regular one for all algorithms (except SolvOpt). For NPSOL, regularization is necessary to obtain convergence. For FSQP, surprisingly, convergence is obtained without regularization when using the Armijo line-search option. However, the regularization improves slightly the convergence time. We observed that regularization allows approximately 20% less collision constraint evaluations. We have no explanation for the good behavior of FSQP for the nonsmooth case. It may be attributed to the special line-search designed to maintain feasible iterates. It would be worth studying this issue from the optimization viewpoint.

Overall, the solver for nonsmooth problems SolvOpt performs worse in terms of the number of iterations but better in terms of computation time than other solvers, because of a smaller computation time per iteration.

These results depend on the optimization problem (see others in Section VIII). We later see that the relative performance of SolvOpt degrades quickly with more complex problems.

This study also suggests an optimal choice of the regularization radius wrt the desired precision [see Fig. 5(c)]. The precision for a given radius depends on the body's shape: bodies with smaller edges and faces give a better precision for the same regularization radius. For a 1-m bar, to obtain a 10 -n precision of (h, θ), the best choice is a circle of radius 10 n m. From the study of the relation between the obtained precision on h and the sphere radius, we observed that the radius providing the fastest convergence is linked to the regularization precision given by Theorem 6.2. Indeed, smaller radii give less precision (bigger bulging), but bigger radii result in stiffer problems, whose convergence is slower. Hence, the best choice of regularization radius is the smallest possible radius that fulfills the desired precision. [START_REF] Gilbert | Distance functions and their application to robot path planning in the presence of obstacles[END_REF] demonstrated the necessity of having one body strictly convex and the other convex to obtain the differentiability of their separating distance function. We recall this theorem and complement it with additional properties.

IV. DISTANCE CONTINUITY PROPERTIES

Gilbert and Johnson

A. Problem Definition and Notations Definition 4.1 (Convexity and Strict Convexity):

An object O is convex iff ∀(X, Y ) ∈ O 2 and ∀t ∈ ] 0, 1 [ the point P defined by P = (1 -t)X + tY lies in O; it is said to be strictly convex if X = Y and P lies in int O, the interior of O.
Let δ be the distance between two convex objects O 1 and O 2 . The relative position between the two objects is locally parameterized by q ∈ IR 6 .

We call witness points a pair of points of O 1 × O 2 that are at the distance δ. Under certain conditions (see later Lemma 4.1), this pair is unique. Thus, we can define p min (q) = (p 1min (q), p 2min (q)) T as the function that associates the pair of witness points with each q.

Furthermore, the surface of each object can be described by a function of two parameters. Let u be the 4-D vector of these 2 × 2 parameters, and r 1 and r 2 be these functions for O 1 and O 2 (to simplify, we define r 1 and r 2 as a function from a subset of IR 4 to IR 3 even though both of them are functions from a subset of IR 2 to IR 3 ).

The witness points being on the objects' surfaces, we define u min as the function of q that returns the vector u of these points. Let R(q) be the rotation matrix, and T (q) the translation vector; we have p min (q) = (r 1 (u min (q)) , R(q)r 2 (u min (q)) + T (q)) T .

Finally, f (q, u) = r 1 (u) -R(q)r 2 (u) -T (q) is the distance vector between the two witness points parameterized by u δ(q) = min u f (q, u)) = f (q, u min (q)) .

(

Note that f is C ∞ wrt q since R and T are rotation and translation, and that if r 1 and r 2 are C k wrt u, so is f .

B. Strict Convexity of a Body

Gradient discontinuity occurs at a jump of the witness points (see Section III). This happens around a configuration in which the witness pair is not unique, e.g., when an edge is parallel to a face. If one object is strictly convex, parallelism does not happen. The following successive lemmas and theorems demonstrated in [START_REF] Gilbert | Distance functions and their application to robot path planning in the presence of obstacles[END_REF] link the uniqueness of witness points (hence the continuity of the gradient) with the strict convexity of one object. Thus far, they are valid for δ > 0.

Lemma 4.1 (Uniqueness of witness points):

There is a unique pair of witness points if at least one of the bodies is strictly convex (and the other is convex).

Lemma 4.2: The witness points of the minimum distance between two convex bodies are continuous functions of q if at least one of the bodies is strictly convex and the other is convex (u min and p min are continuous functions of q). Theorem 4.3: The minimum distance between two convex bodies is a C 1 function of q if at least one of the bodies is strictly convex and the other is convex.

If the surfaces of these objects have additional continuity properties and ∂ 2 f 2 ∂ u 2 is invertible (which is always the case but for pathological cases), the distance function becomes smoother.

Lemma 4.4: If the surfaces of both bodies are C k , with k ≥ 2, and one body is strictly convex, then the witness points are C k -1 functions of q.

Proof: Let u 0 be the coordinates of the witness points at q 0 .

We have ∂ f ∂ u (q 0 , u 0 )

T f (q 0 , u 0 ) = 0 [optimality condition of the minimization problem in (3)] which can be rewritten as

∂ f 2 ∂ u (q 0 , u 0 ) = 0. Let F (q, u) = ∂ f 2 ∂ u (q, u). F is C k -1 , F (q 0 , u 0 ) = 0 and ∂ F ∂ u (q 0 , u 0 ) is invertible. Thus, u is locally a C k -1 function of q (implicit functions theorem). Subsequently, u min is a C k -1 function of q.
Theorem 4.5: If the surfaces of both bodies are C k , with k ≥ 2, and one body is strictly convex, then the minimum distance between them is C k .

Proof: Let us derive δ(q) = f (q, u min (q)) wrt q ∂δ ∂q (q) = ∂f ∂q

+ ∂u min ∂q • ∂f ∂u T f f = ∂f ∂q (q, u min (q)) T f (q, u min (q)) f (q, u min (q)) (4)
since ∂ f ∂ u (q, u min (q)) T f (q, u min (q)) = 0. Using Lemma 4.4, the demonstration is straightforward. To sum up, the continuity properties of the minimum distance function with respect to the relative position of two convex bodies are as follows.

1) The distance is always C 0 (even with no convexity assumptions). 2) It is piecewise C 1 with simple convexity. Discontinuities of the gradient arise when faces or edges are parallel (nonstrict convexity). 3) Strict convexity of a body and C 0 surfaces ensures the C 1 property of the distance. Having C 1 surfaces does not improve the continuity of the distance as compared to having C 0 surfaces, 4) Additional C k property of both surfaces yields C k smoothness of the distance if ∂ 2 f 2 ∂ u 2 is invertible. These properties are also true locally. In particular, if witness points move on the interior of C ∞ surfaces, one at least being strictly convex, the distance is C ∞ . This is the case when considering the distance between two spherical parts.

C. Interpenetration Case

The C n property for n > 0 cannot be reached everywhere in the interpenetration case; the distance minimization problem is not convex anymore. Indeed, in some configurations there are several (up to infinite, see Fig. 6) pairs of witness points. Jumps between witness pairs are thus inevitable resulting in gradient discontinuities. However, we can retain the results of the aforementioned theorems for a subset of penetration cases. But first we need to ensure the continuity properties between the penetration and the nonpenetration case. We define the penetration distance as the opposite of the distance between the pair of points verifying the optimality condition ( ∂ f ∂ u ) T f = 0 while being at the minimal distance among the possible pairs with opposite normal vectors (which is the same definition as in the nonpenetration case). This definition is equivalent to the minimum separating translation distance (see [START_REF] Van Den Bergen | Collision Detection in Interactive 3-Environments[END_REF]Ch. 2]). Jumps between pairs do not happen if the penetration depth is less than twice the minimal radius of curvature of the penetrating parts of the objects. Under this assumption of "slight" penetration, the previous results hold; if not we may then have gradient discontinuities. In the worst case, we may encounter the discontinuities for δ = 0 when at least one witness point is on a vertex and the other on an edge. These are cases of bifurcation of the number of witness pairs. However, these discontinuities will be avoided when a security margin is considered for collision avoidance [START_REF] Gilbert | Distance functions and their application to robot path planning in the presence of obstacles[END_REF], which is the case for our bounding volume (see the next section). Therefore, discontinuities will only occur in strict penetration cases, which are repulsive situations: the gradient points away from them. Hence, they should not be a problem in most applications where penetration can occur (like in optimization starting with an unfeasible point).

V. SPHERE-TORUS-PATCH BOUNDING VOLUMES

A. Definition:

We consider a point cloud P, and, for r ≥ 0, the set P r of (closed) balls B(P, r) centered at each P ∈ P and with radius r (P 0 ≡ P). Let B R,r (P) be the set of all balls with radius R that encloses every ball of P r (Rr must be at least the radius of the smallest sphere containing P).

Definition 5.1 (STP-BV) We name STP-BV of P for the radii R, and r the intersection of all the balls of radius R containing the balls of radius r centered at each point of P STP R,r (P) = B ∈B R , r (P)

B.

(5) Henceforth, we ignore the special case where there is a single point in P and r = 0 (therefore, the STP-BV has a nonempty interior).

B. Strict Convexity

We now show that an STP-BV is strictly convex, which is not straightforward. Indeed, the intersection of an infinite number of convex volumes is always convex, but the intersection of an infinite number of strictly convex volumes is not necessarily strictly convex. We prove with the following lemma and theorem that the strict convexity is retained in the case of STP-BV. The lemma shows that for a point strictly on a segment, a minimal distance to the boundary of any ball with radius R containing this segment can be guaranteed. Proof: O being not fixed, the longest distance between O and M is achieved when O is such that X and Y are both on the surface of B(O, R): OX = OY = R. 2 We consider this case and we denote the middle point of the segment [X, Y ] as I (see Fig. 7). The triangle OIX is orthogonal at I so that OI 2 = R 2 -1 4 XY 2 . Then considering the triangle OIM, orthogonal at I, we have

OM 2 = R 2 -t(1 -t)XY 2 . We have t(1 -t)XY 2 > 0 since t ∈ ] 0, 1 [ and X = Y , so that OM < R.
In the general case (i.e.,

OX ≤ R, OY ≤ R), we have OM ≤ R 2 -t(1 -t)XY 2 < R so that whatever O, M is at least at a distance ρ > 0 of the boundary of B(O, R).
Theorem 5.2 (Strict Convexity of STP-BV): STP R,r (P) is strictly convex.

Proof: Let us show that for any X = Y of the STP-BV and any

t ∈ ] 0, 1 [ , (1 -t)X + tY ∈ int STP R,r (P). Let X = Y . If X, Y ∈ STP R,r (P), then for every B of B R,r (P), X, Y ∈ B.
In particular, XY ≤ 2R. We define t, M , and ρ as in Lemma 5.1. Then, ∀B ∈ B R,r (P), int B(M, ρ) ⊂ int B; thus int B(M, ρ) ⊆ int STP R,r (P) so that M ∈ int STP R,r (P).

Fig. 8. STP-BV bulge tuning with the two radii (R, r). The parts of big spheres are depicted in dark gray, the tori in lighter gray, and the small spheres in black. R m in denotes the radius of the smallest bounding sphere. In this example, it is 0.87 + r. For (R, r) → (∞, 0), the STP-BV tends to the convex hull of the original polyhedron (bottom right). An increase in r has a dilating effect, whereas a decrease in R from the convex hull (∞, 0) to the enclosing sphere (R m in , 0) achieves continuous bulging. Fig. 9. Sphere construction. C is equidistant from each P i , which means its projection is the circumcenter of the face (at the intersection of the edge); perpendicular bisectors are depicted by thin dash lines.

C. Surface

Here, we describe the surface of an STP-BV. The following theorem helps us reducing the amount of work.

Theorem 5.3 (Dilation): STP R,r (P) is the dilation of STP R -r,0 (P) by a ball of radius r.

Proof: This stems trivially from the fact that if X is in a ball of radius Rr, then B(X, r) is included in the ball having the same center, and radius R, and conversely.

For presenting how to build an STP-BV surface, we take r = 0. For the sake of simplicity, we write R instead of Rr.

A ball B = B(C, R) ∈ B R,0 (P) really limits the STP-BV if it is tangent to one or more points of P. The following three cases are observed.

1) B is tangent to three or more points P i (see Fig. 9). The part of the corresponding sphere, which is inside the cone defined by the vectors CP i and with apex C, is a part of the boundary of the STP-BV. 2) B is tangent to two points P 1 and P 2 . The geodesic arc on B from P 1 to P 2 is part of the boundary of the STP-BV. Let B 1 be a ball tangent to P 1 , P 2 and an additional point P 3 . All the balls of B R,r (P) tangent to P 1 and P 2 are obtained by rotating B 1 around P 1 P 2 and away from P 3 until it becomes tangent to a new point P 4 . The corresponding geodesic arcs form a part of torus (see Fig. 10), whose limits are the planes C 1 P 1 P 2 and C 2 P 1 P 2 , where C 1 denotes the center of B 1 and C 2 denotes the center of the ball tangent to P 1 , P 2 , and P 4 . 3) B is tangent to only one point. It limits the STP-BV only at this point. The surface of the STP-BV is thus made of parts of spheres and tori, hence the name sphere-torus-patch bounding volume. The sphere parts can be seen as the "faces" of the volume and the tori as its "edges." The vertices are the points of P that define the axes of the tori. When r > 0, the vertices become parts of small spheres with radius r; see Fig. 8.

By definition, STP R,0 (P) is the smallest bounding volume built with balls of radius R. H(P) being the convex hull of P, we have H(P) ⊂ STP R,0 (P). The parameter R denotes the maximal radius of curvature of the STP-BV and controls the boundary on the maximal margin between the STP-BV and H(P) (see Theorem 6.2). When R tends to infinity, STP R,0 (P) tends to H(P). The STP-BV can thus be made arbitrarily close to a convex polyhedron. When used, the parameter r gives the minimal distance between STP R,r (P) and H(P). It is used as a security margin. Both R and r are set by the user.

VI. GEOMETRIC CONSTRUCTIONS

The vertices and torus axes of STP R,0 (P) define a polyhedron H R (P) (usually but not necessarily convex). To each B ∈ B R,0 (P) tangent to three or more points of P corresponds a polygonal face of H R (P) defined by these points (the face may not be planar in the rare case of nonplanar cospherical points). If a face has more than three points, we divide it into triangles. The new edges correspond to flat tori. Obviously, H R (P) ⊆ H(P). We show how to build spheres and tori from edges and faces and prove some properties.

A. Sphere Construction

Let us consider a triangular face of H R (P) (see Fig. 9). P 1 , P 2 , and P 3 are its vertices given counterclockwise around the outer normal.

We want to build the (unique) sphere of center C and radius R that goes through the three vertices of the face, and is above the face (direction given by the outer normal).

Let u = P 1 P 2 , v = P 1 P 3 , c = P 1 C, and w = u × v. w is collinear to the outer normal and points in the same direction.

C needs to be equidistant to the points P i and is thus on the median planes of the edges. We then solve the following system to find the coordinates of C in the frame (P 1 , u, v, w):

⎧ ⎪ ⎨ ⎪ ⎩ u.c = u 2 /2 (median plan of P 1 P 2 ) v.c = v 2 /2 (median plan of P 1 P 3 ) c 2 = R 2 c.w < 0 (inner solution). (6)
Writing c = αu + βv + γw, we get from the two first equations

α = u 2 v 2 -u.v • v 2 2(u × v) 2 , β = u 2 v 2 -u.v • u 2 2(u × v) 2 .
These are the coordinates of the circumcenter of the triangle.

Replacing α and β in the third equation leads to an equation of degree 2 in γ. Only one of its two solutions complies with the fourth equation:

γ = -R 2 -α 2 u 2 -β 2 v 2 -2αβ u.v (u ×v ) 2
. Of the sphere centered on this point C, we only keep the part inside the cone defined by vectors CP i and whose apex is C.

B. Torus Construction

We obtain the torus above one edge of H R (P) by rotating the sphere of an incident face around this edge and keeping the resulting inner volume. The construction, depicted in Fig. 10, is based on the following result.

Lemma 6.1: The distance between the center of a sphere corresponding to a face and the median point I of one of its edges depends only of the length l of this edge and is R 2 -l 2 4

Proof: With the notation of Fig. 10, it is the direct result of the pythagorean theorem written for the triangle (IC 1 P 2 ).

The centers C 1 and C 2 of the two spheres corresponding to the two incident faces of the edge that we consider are thus on a same circle with center I and radius R 2 -l 2 4 . We consider the circle C 1 of center C 1 and radius R in the plane defined by C 1 and the edge. By construction, this circle coincides with the sphere centered in C 1 .

By making this circle revolve around the edge until it coincides with the circle C 2 of same radius centered in C 2 and in the plane defined by C 2 and the edge, we obtain the part of torus we need. Note that the torus does not have the usual shape of a donut since its usual small radius is bigger than the (usual) big one. The part that we consider here is on the inner side of the entire torus, as shown in white in Fig. 11. The torus and the spheres coincide in their delimiting planes and are perpendicular to these planes. Therefore, the torus is tangent to both spheres, and the junction between the torus and each sphere is then C 1 .

C. Properties Theorem 6.2 (Maximal margin):

If a denotes the length of the longest edge of the polyhedron, then the maximal margin between H R (P) and STP R,0 (P) is R -R 2 -a 2 12 . Proof: Since each edge of H R (P) is the revolution axis of its associated torus, the maximal margin is achieved in its median plane, and is equal to R -R 2 -l 2 4 , l being the length of the edge.

For a face, the following two cases are observed with respect to the position of the circumcenter H:

1) H is outside of the face, and then, the maximal margin is achieved on the longest edge of this face; 2) H is inside the face. In this case, for a fixed longest length of the edges, the maximal margin is achieved when the face is equilateral and is R -R 2 -a 2 12 . Since H R (P) ⊆ H(P), this is also the maximal margin to H(P). Theorem 6.3 (Continuity property of STP-BV): When r > 0, the surface of the STP-BV is C 1 .

Proof: (sketch) For r = 0, the STP-BV is convex and its surface is C 1 everywhere but at its vertices. A dilation of r > 0 makes it round at the vertices, thus C 1 everywhere.

An STP-BV is even piecewise C ∞ since the tori and the spheres are C ∞ surfaces. However, as stated in Theorem 4.5, there is no advantage in having C 1 surfaces instead of C 0 with respect to the distance continuity.

VII. SPHERE-TORI-PATCHES BOUNDING VOLUME CONSTRUCTION

There is by definition a one-to-one relation between the spheres and tori of the STP R,0 (P) and the faces and edges of H R (P). The construction of H R (P) from P is described in Algorithm 1. It is mostly a gift wrapping algorithm with spheres instead of planes (see Fig 12) 3 ; we find an initial face whose associated sphere contains P. We then rotate the sphere around the edges of the face until it reaches new points of P. This defines new faces and new edges around which to rotate.

The init function (Algorithm 2) finds an initial face. Since we can perform only one rotation at a time, we maintain a list of rotations to be completed. Maintaining this list next is the main task of Algorithm 1. For a reason we provide later, the smallest of the rotations in this list is performed first. An element of next is a set (v 1 , v 2 , v 3 , (v 4 , a)) encoding that the (positive) rotation of the sphere associated with the face v 1 v 2 v 3 around the edge v 1 v 2 reaches the point v 4 after a rotation of angle a (in Fig. 10, a is the angle between IC 1 and IC 2 ). 3 See also the supplementary multimedia material.

The list next is ordered according to the angles a. It supports the classical operations pop and insert. The operation contains(v 1 , v 2 ) returns true if any element of next begins by (v 1 , v 2 ) or (v 2 , v 1 ). delete(v 1 , v 2 ) removes such an element. output is a structure describing a polyhedron (sets of vertices, edges, and faces). The function addFace(v 1 , v 2 , v 3 ) updates it by adding the face v 1 v 2 v 3 (described counterclockwise) and the corresponding edges and vertices, if not already present. sphere(v 1 , v 2 , v 3 , R) returns the center of the sphere with radius R as computed in Section VI-A. angle(c 1 , c 2 , v 1 , v 2 ) returns the angle between ic 1 and ic 2 , where i is the middle point of v 1 v 2 ; nextPoint(v 1 , v 2 , v 3 ) looks for the first point v encountered while rotating the sphere associated with v 1 v 2 v 3 around v 1 v 2 , and returns it along with the rotation angle made to reach it. v denotes the only point for which the sphere associated with v 1 vv 2 contains P, but when there are cospherical points, in which case any of these points will do. The special case where v = v 3 is discussed later.

At each iteration, Algorithm 1 processes a new face v 1 vv 2 obtained by turning around v 1 v 2 . For the two edges v 1 v and vv 2 the following two cases can be observed.

1) The edge was encountered before when processing a face f . There is no need to rotate around this edge anymore because rotating the sphere associated with f around the edge would give v 1 v v 2 and vice versa. The rotation is removed from next.

2) The edge was not encountered before. We need to perform the rotation around it later so it is added to next. The output of Algorithm 1 is H R (P) from which, with the computations described in the previous section, we can build the boundary of STP R,0 (P) (spheres and tori with their limits) and thus any STP R +r,r (P).

The reason for choosing the smallest rotations first is to ensure the robustness of the algorithm wrt numerical errors in the case of polygonal faces whose vertices are on the same sphere (cospherical). In this case, all vertices should be reached at the same time when turning around an edge of this face, and thus, the vertex with the lowest index is selected. However, this is sometimes not the case because of numerical rounding errors. Thus, different vertices can be chosen when we reach the face by turning around different edges, resulting most of the times in overlapping triangles, which results in an algorithm failure.

To avoid this, we force the algorithm to finish covering a polygonal face that it already began, by choosing to turn around the edge with the lowest rotation. This avoids us setting a threshold that defines when points are cospherical and searching for cospherical points each time there is a rotation around an edge.

In special cases, it can happen that the rotation of the face

v 1 v 2 v 3 around v 1 v 2 stops at v = v 3 .
For example, this occurs when P has only three points, but it can happen with more. 4 In this case, after turning around v 1 v 2 , the algorithm will remove the rotations around v 1 v 3 and v 2 v 3 from next, while one of them might still be needed. To avoid this, nextPoint forces the angle to π when detecting the case, to ensure that the corresponding rotation will be the last one processed.

Finally, init fails in two cases: 1) if R is smaller than the radius of P (STP R,0 (P) does not exist), and 2) in the case of thin long clouds, where no ball of B R,0 (P) is tangent to three points of P. In the latter case, STP R,0 (P) is the torus whose axis is defined by the two furthest points of P.

Remark (computation time): For the polyhedrons that we usually consider (few hundred vertices), the construction time is around 1 s. The full STP-BV model of the robot discussed in Section VIII-B, is obtained in approximately 30 s.

VIII. APPLICATIONS

Distance computation algorithms can be categorized as feature-based and simplex-based. In [START_REF] Escande | Continuous gradient proximity distances for humanoids free-collision optimized-postures generation[END_REF], we implemented a featured-based distance computation for the STP-BV based on V-Clip [START_REF] Mirtich | V-Clip: Fast and robust polyhedral collision detection[END_REF]. Later (see [START_REF] Benallegue | Fast C 1 proximity queries using support mapping of sphere-torus-patches bounding volumes[END_REF]) we implemented a simplex-based method based on the GJK algorithm [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimentional space[END_REF] [24] and its adaptation for the case of penetration distance [START_REF] Ong | Growth distances: New measures for object separation and penetration[END_REF]. Here, we use the latter solution because the penetration depth is necessary in optimization algorithms working with nonfeasible iterates (such as most of the gradient-based algorithms, like NPSOL). In a nutshell,

TABLE II MEAN TIME PER ITERATION IN MICROSECOND (WITH STANDARD DEVIATION)

FOR THE OPTIMIZATION PROBLEM ( 7)- [START_REF] Zhang | k-IOS: Intersection of spheres for efficient proximity query[END_REF] the integration of the STP-BV in a GJK scheme boils down to writing its support function s, i.e., the function that maps a vector v to the extremal point p of the STP-BV in the direction of v. Let f be a part of a torus or sphere defining the surface of the STP-BV. All the normal vectors to f form a cone N f . We have s(v) = s f (v) with s f the support function of the torus or sphere supporting f such as v ∈ N f (see [START_REF] Benallegue | Fast C 1 proximity queries using support mapping of sphere-torus-patches bounding volumes[END_REF] for more details).

The support function is computed in O( √ n), where n denotes the number of vertices in H R (P). However, it comes close to O(1) when there is a temporal coherency. Our STP-BV library5 consists of two parts: the STP-BV construction from a point cloud, and the distance computation, which can be used like any classical distance computation algorithms. One can express collision avoidance constraints by requiring that the returned distance is positive, or enforce contacts by imposing that the distance must be equal to zero.

A. Cube Problem

We study a generalization of the problem ( 1)-( 2) to the 3-D case: minimizing the distance of a six -DOF cube to a reference configuration while avoiding a collision constraint with a fixed bigger cube (see Fig. 13). The fixed big cube is modeled as a polyhedron and the small one as a polyhedron or its STP-BV regularization. The optimization problem solved is as follows:

min x C 2 2 -tr(R(ψ 0 , θ 0 , φ 0 )R(ψ, θ, φ) T ) (7)
s.t. d(x) ≥ d 0 (8)
where x = [C x , C y , C z , ψ, θ, φ] with C = [C x , C y , C z ] T as the cube center, and ψ, θ, φ denote the roll, pitch, and yaw angles respectively, of the cube orientation, ψ 0 , θ 0 , φ 0 denote the reference orientation. R(., ., .) denotes a rotation matrix, tr(.) indicates for the trace, and d(.) denotes the distance between the small and the big cubes. The minimum of -tr(R(ψ 0 , θ 0 , φ 0 )R(ψ, θ, φ) T ) is obtained when both rotation matrices are equal, which gives a value of -3.

The computations performed are the same as with the bar problem, and we chose (ψ 0 , θ 0 , φ 0 ) to make the optimization converge either to a regular point (see Fig. 13, first case) or to a kink point (see Fig. 13, third case). Similarly to the bar problem, despite the kink points of the problem being a subset of measure zero in the parameter space, the optimization will converge to them for regions of the (ψ 0 , θ 0 , φ 0 ) space that have nonzero measure. The probability of converging to a kink point is thus not zero. Results are given in Fig. 14 and Table II. We did not run NPSOL without regularization because of the poor convergence obtained in the bar problem. One can observe that convergence is faster to a regular point. In terms of the computation time, SQP algorithms are faster than SolvOpt, whereas in terms of the number of iterations, SQP algorithms are considerably better. In terms of the the computation time, the fastest convergence to a kink point is obtained with FSQP using the STP-BV, followed by the use of NPSOL with the STP-BV for high precision and FSQP with the cube (i.e., not using the STP-BV) for low precision; in terms of the number of iterations, the fastest algorithm is NPSOL, followed by FSQP with the STP-BV, and FSQP without the STP-BV. At a regular point, in terms of the computation time, the fastest is FSQP without the STP-BV, closely followed by FSQP with the STP-BV and NPSOL; in terms of the number of iterations NPSOL is better, followed by FSQP. A noteable difference with the bar case is that the use of the STP-BV significantly improves the convergence speed of FSQP that can however correctly converge to a kink point. As in the bar case, the best choice of radius is the smallest one that gives the required precision.

B. Collision-Free Humanoid Posture Generation

Our study is originally motivated by the integration of collision avoidance in a planning algorithm presented in [START_REF] Escande | Planning contact points for humanoid robots[END_REF], part of which consists of generating postures. We show results from our posture generator using the STP-BV for collision avoidance. In brief, this posture generator performs an optimization under constraints such as static equilibrium, required body positions (for example, robot-environment contacts, described as matches between frames of the robot and frames of the environment), and collision avoidance. The variable is the configuration vector. The criterion to be minimized can be any user-defined smooth function. In the following scenarios, we use the distance to a human-like upright position.

The problem of collision avoidance in posture generation was addressed in [START_REF] Abdel-Malek | Towards a new generation of virtual humans[END_REF] and [START_REF] Peinado | Progressive cartesian inequality constraints for the inverse kinematic control of articulated chains[END_REF]. Abdel-Malek et al. [START_REF] Abdel-Malek | Towards a new generation of virtual humans[END_REF] generated postures from an optimization formulation. Spheres are used as coverage to define the collision avoidance constraints. Their method requires a considerable number of spheres to cover the virtual avatar and hence a considerable number of pairwise constraints. Peinado et al. [START_REF] Peinado | Progressive cartesian inequality constraints for the inverse kinematic control of articulated chains[END_REF] generated postures from inverse kinematics and prioritized tasks, but this method is not smooth with all observers' primitives; moreover, observers need to be specified by the user and are context specific. There are other approaches to the aforementioned problem such as in [START_REF] Sentis | Synthesis of whole-body behaviors through hierarchical control of behavioral primitives[END_REF] where avoidance is made through potential fields.

The model of each HRP-2 body contains between 50 and 800 vertices. Parameters of the STP-BV are as follows: r = 2 cm, and R is fixed between 1 m and 10 5 m. The environment is modeled with polyhedrons, as only one body of a pair needs to be strictly convex for the continuity of the distance gradient. Body pairs that need to be checked for self-collision have been studied in [START_REF] Kuffner | Self-collision detection and prevention for humanoid robots[END_REF] and [START_REF] Okada | A hybrid approach to practical self collision detection system of humanoid robot[END_REF]. In the latter, lookup tables are used for dealing with composed joints; hence, the safety margins of the bounding volumes do not restrain the movement possibilities. We focused on this point too, but since we need continuous gradients for all constraints, such a method was not possible. Thus, we used specific analytical functions of the joints values to prevent collision around the hip, waist, neck, and shoulder joints. These functions were obtained either geometrically or by experimentations on a real HRP-2 robot. There are 117 autocollision constraints. Eight of them are analytical, the others being computed with the STP-BVs. We apply this posture generator to two scenarios (see also [START_REF] Escande | Continuous gradient proximity distances for humanoids free-collision optimized-postures generation[END_REF]):

Enter-car scenario: The robot has to place its right foot inside a car, on the floor in front of the driver seat, and its left foot on the floor outside of the car. The robot must be stable with only the left foot contact. Nineteen pairs are checked for collisions with the environment. The scenario is illustrated in Fig. 19.

Reaching scenario: HRP-2 attempts to reach an object near its waist. Both feet are placed on the ground and the left-hand position and orientation are fixed around the object. This scenario was designed to exhibit a nonsmooth self-collision between the forearm and the flat part of the waist, as can be seen in the right picture of Fig. 20. For these two scenarios, we modified our protocol; we identify beforehand the log-linear relations between the parameters of the different algorithms and the radii. Each parameter p i is chosen to follow a law p i (R) = 10 a i log 1 0 R +b i , where the (a i , b i ) have been identified to obtain the best iteration-precision ratio for a set of scenarios. We then run both scenarios above for R varying from 1 to 10 5 ; each time the parameters are given by the laws. Figs. 15-18

TABLE III MEAN TIME PER ITERATION IN MILLISECOND (WITH STANDARD DEVIATION)

FOR Enter-car AND Reaching SCENARIOS can then be seen as parametric functions of R. We proceed this way to get closer to real-condition uses; we do not take the results for the best parameters and radii tuned for each particular scenario-such a tuning cannot be considered when generating thousands of different postures, e.g., in our case of planning. Each scenario is running with the same parameters, chosen to have the best compromise between the computation time and the expected precision of the solution. The initial postures are randomly chosen around the straight posture within a range of 4 • for the angles and 6.5 cm for the root coordinates. Figs. 15-18 and Table III present the computation results for the Enter-car and Reaching scenarios. In each case, the results are given with and without the use of the STP-BV using NPSOL and FSQP (we did not run SolvOpt because of its poor results in the cube case). The displayed results are the mean values of 100 runs with random initial conditions, for each value of R. Despite the short range of initial values, the algorithms sometimes converge to different local minima that we identified manually (for example, the left arm above the car door for the Enter-car scenario, instead of being along it as seen in Fig. 19). For each of them, we compute the best parameter vector x ref over an extensive series of runs. The convergence precision is then measured as the distance between the obtained parameter vector x of a run and its closest local minimum. At the optimum of the Enter-car scenario, six collision constraints with the environment are active. Only one of them is nonsmooth at the solution. At the optimum of the Reaching scenario, one collision constraint is active (between the waist and the left arm) which is nonsmooth at the solution. The convergence ratios are presented in Figs. 15 and 17, and for the cases which converged, Figs. 16 and 18 illustrate the convergence speeds.

For both scenarios, FSQP allows a better convergence ratio than NPSOL. Nonconvergences are due to the impossibility of finding a feasible solution. On average, for NPSOL, the use of STP-BV decreases the number of iterations by approximately 50% and the computation time by approximately 25%. On average, for FSQP, the use of the STP-BV does not significantly improve the number of iterations, and the computation time is slightly slower, but it allows reaching better mean precisions. For both FSQP and NPSOL, the confidence ellipses are smaller with the STP-BV, representing a better precision reliability of the obtained solutions. The use of the STP-BV significantly improves the convergence speed of NPSOL because a nonsmooth collision is active at the solution, which leads to convergence problems without STP-BV. However, as compared to the cube problem for which no convergence is obtained without STP-BV, here, convergence is obtained because the nonsmoothness is relatively not dominant; indeed, the size of the face causing gradient discontinuity is considerably smaller than the cube's. For FSQP, we do not have explanations why the nonsmoothness has so little impact on the convergence speed, except that it might have something to do with the feasibility of the iterates. Our interpretation of the nonconvergence due to unfeasibility with NPSOL is that the nonlinear problems that we attempt to solve are highly nonconvex, particularly because of the collision constraints. The reason why FSQP accommodates better with this nonconvexity is because it keeps feasible iterates, what may help to avoid unfeasible local minima in which NPSOL falls. Another consequence of the nonconvexity of the problem is that in approximately 10-40% of the convergence cases with NPSOL, the convergence was obtained in other local minima. Even if the problems

IX. CONCLUSION

We propose a new bounding volume operator to enforce the continuity of the gradient of the distance function. The main idea is to ensure the strict convexity of the bounding envelopes of the objects, which are computed offline. We proposed such a bounding volume made as a patch of parts of spheres and tori, the STP-BV, whose parameter choice is a tradeoff between the tightness of the volume and the regularization of the gradient of the distance. We showed that this regularization together with classical smooth optimization is an interesting alternative to the direct use of a nonsmooth approach, through academic cases and a collision-free (including self-collision) optimization-based humanoid posture generation for HRP-2.

Fig. 1 .

 1 Fig. 1. Segment free to rotate around one of its points. d denotes the distance to the horizontal axis.

Fig. 2 .

 2 Fig. 2. When θ passes over 0 (b), the witness points jump between the configurations of (a) and (c). (a) θ < 0. (b) Parallel case: θ = 0. (c) θ > 0.

Fig. 5 .

 5 Fig. 5. Comparison of the number of iterations (ordinates) required to achieve a desired precision of the result (abscissa with x = [h, θ]) for the optimization problem (1)-(2) with (a) d 0 = 0.05 and θ b = 0.01 or (b) θ b = 0.02, and (c) corresponding regularization radius. One can note that when converging to a regular point, FSQP behaves exactly the same with or without regularization. Furthermore, there is no real relation between the optimal radius and the precision when converging to a regular point, which is normal since regularization is not an issue at this point. (a) Convergence to a regular point. (b) Convergence to a kink point. (c) Best radius for a given precision.

Fig. 6 .

 6 Fig.6. For two objects in interpenetration (one in the plain line, one in the dash line), a degenerate case where there are an infinity of witness pairs, some of which are depicted in gray.

Fig. 7 .

 7 Fig. 7. Figure for Lemma 5.1.

Lemma 5 . 1 :

 51 Let X and Y be two points such that X = Y and XY ≤ 2R. Let t ∈ ] 0, 1 [ and define M = (1t)X + tY and ρ = R -R 2t(1t)XY 2 . Then, for any ball B(O, R) that contains X and Y, int B(M, ρ) ⊂ int B(O, R) and int B(M, ρ) is not empty.

Fig. 10 .

 10 Fig.10. Torus construction. The gray plane indicates the median plane of the edge P 1 P 2 , on which are depicted its intersection with the incident faces, the corresponding spheres, and the torus in between. The circles C 1 and C 2 denote the limits between the torus and the spheres (not depicted here) and generate this torus by rotation around (P 1 P 2 ).

Fig. 11 .

 11 Fig. 11. Black and white parts are the portions of torus that we consider; the white surface is the part used in the STP-BV.

Fig. 12 .

 12 Fig. 12. Construction of the STP-BV of a cloud P in a 2-D case by rotation of an initial circle (left) around the points as it encounters them. Some points of the convex hull H(P) (gray polygon) are dismissed due to the curvature of the circle. The STP-BV polygon H R (P) is depicted by a thin dashed line (right).

Fig. 13 .

 13 Fig. 13. Cube problem. From left to right: (a) case with only one pair of witness points, (b) one edge parallel to a face of the big cube, inducing an infinity of witness pairs, (c) parallel faces, and (d) the STP-BV of the cube in the same configuration as (c), yielding a single pair of witness points.

Fig. 14 .

 14 Fig. 14. Comparison of the computation time (ordinate) to achieve a desired precision of the result (abscissa) for the optimization problem (7) and (8), and the corresponding optimal regularization radius. FSQP behaves the same with or without regularization when converging to a regular point. (a) Convergence to a regular point. (b) Convergence to a kink point. (c) Best radius for a given precision.

Fig. 15 .

 15 Fig. 15. Enter-car scenario. Percentage of convergence (ordinate) achieved for a desired precision of the result (abscissa).

Fig. 16 .

 16 Fig. 16. Enter-car scenario. Comparison of the mean iteration number (ordinate) to achieve a desired precision of the result (abscissa). Confidence ellipses represent the repartition of solutions around an arbitrary mean point.

Fig. 17 .

 17 Fig. 17. Reaching scenario. Percentage of convergence (ordinate) achieved for a desired precision of the result (abscissa).

Fig. 18 .

 18 Fig. 18. Reaching scenario. Comparison of the mean iteration number (ordinate) to achieve a desired precision of the result (abscissa). Confidence ellipses represent the repartition of solutions around a mean point.

Fig. 19 .

 19 Fig. 19. Enter-car scenario. From left to right: without collision avoidance constraints, successful collision-free posture from three different view points.

Fig. 20 .

 20 Fig. 20. Reaching scenario. (Left) Posture obtained without collision avoidance constraints. (Right) Successful collision-free posture.

TABLE I MEAN

 I TIME PER ITERATION IN MICROSECONDS (WITH STANDARD DEVIATION) FOR THE OPTIMIZATION PROBLEM (1)-(2)

http://www.sbsi-sol-optimize.com/asp/sol_product_npsol.htm

This is quite intuitive, but it can be proven by writing the maximization of OM wrt O, with the constraints OX ≤ R and OY ≤ R.

When for three points v 1 , v 2 , and v 3 both spheres built on v 1 v 2 v 3 and v 1 v 3 v 2 contain P.

STP-BV library can be provided by authors upon request.

This work was partially supported by the European Commission's Seventh Framework Program as part of the ROBOHOW.COG project under Grant 288533. A. Escande is with the Centre National de la Recherche Scientifique-National Institute for AIST JRL UMI3218/CRT), Japan