Kai Henning Koch

Katja Mombaur

Olivier Stasse

Philippe Souères

Optimization based exploitation of the ankle elasticity of HRP-2 for overstepping large obstacles

This paper proposes a new generic strategy to investigate the dynamic limits of the humanoid robot HRP-2 based on whole body optimal control optimization. In this study we exploit the intuitive access to complex motion characteristics, given by optimal control, to effectively resolve a major technical coupling effect, namely between the ankle elasticity and the stabilizing algorithms. Control efforts are reduced to get a clearer view of the actual system limits and to exploit its capacities at maximum. As showcase we decided to focus on a stepping motion over a cylindrical obstacle. This study is further supported by real experiments on the HRP-2 14 robotic platform and we could successfully extend the present maximum of a dynamically overstepped obstacle to 20cm (height) x 11cm (width) (including safety margin) without multi-contact support.

I. INTRODUCTION

While humanoid robots are some of the most exciting machines in the world, they come at the same time with a highly complex design of its overall system architecture. At one hand this complexity is essential to the capacity of bipedal locomotion (e.g. redundancy & under-actuation). At the other hand, it poses further difficulties as soon as one tries to investigate or even improve its motion characteristics towards more human-like/natural locomotion trajectories. Even though Humanoid robots have been conceived in the hope to efficiently exploit the advantage of bipedal locomotion to robustly cope with irregular terrain conditions, these robots are eventually struggling with serious problems, as soon as it comes to more challenging situations like, stepping over large obstacles. The possibility to overcome large obstacles is a particular advantage of legged, and especially bipedal systems over their wheeled counterparts. Limitations to a flexible and robust operation for these robots are at some point certainly related to the given hardware and the control system. Besides, some limits may as well originate from the employed algorithms for motion generation and the technological inter-dependencies between control system and the underlying hardware. As some of the frequently employed well-performing algorithms for computing stepping trajectories are very conservative, full exploitation of the capabilities of hardware and control system may not be possible at some point. 1 henning.koch at iwr.uni-heidelberg.de 2 katja.mombaur at iwr.uni-heidelberg.deORB

A. Related research

The research field of motion generation based upon a physical model -more specifically walking motions (see [START_REF] Xiang | Physics-based modeling and simulation of human walking: a review of optimization-based and other approaches[END_REF]) -has the common target to efficiently identify the most suitable motion trajectory, among a sub-manifold of feasible candidate solutions, with the desired characteristics (e.g. stability). Depending on the desired characteristics of the final generation algorithm one may adopt different simplifications (e.g. on the physical model -inverted pendulum [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF]) and heuristics to lower the complexity of the generic problem formulation. On one hand, a very successful heuristic method for stable walking motions is the ZMP-based [START_REF] Vukobratovic | On the stability of anthromomorphic systems[END_REF] pattern generation [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF]. From a desired ZMP trajectory and planned footsteps, based on a simplified model (e.g. 3D-LIPM), one may compute the trajectory of the center of mass and, with further assumptions, the whole body motion (e.g. by inverse Fig. 2. image sequence of the stepping motion kinematics or resolved momentum control). Apart from the ZMP based pattern generation, Englsberger et. al. [START_REF] Englsberger | Three-dimensional bipedal walking control using divergent component of motion[END_REF] have introduced a method featuring the capture point. Most of these pattern generators are easily parametrized, re-actively controllable, but are implicitly bound to the previously made assumptions in form of model-simplifications and heuristics. How these limitations finally translate to the resulting motion is however not always clear. On the other hand, optimization based approaches keep a more generic form of the problem formulation. In consequence their solution generally involves the efficient computation of non-linear programming problems. Thus the high mathematical complexity of these approaches does not permit motion generation in real-time, but gives a more intuitive access to desired motion characteristics based on high-level objectives. Early works on gait cycle optimization based on forward dynamics of whole body optimal control have been conducted by [START_REF] Roussel | Generation of energy optimal complete gait cycles for biped robots[END_REF]. Mombaur et.al. [START_REF] Schultz | Modeling and optimal control of human-like running[END_REF] investigated human-like running based on whole body optimal control using multiple shooting techniques. The stack of tasks [START_REF] Ramos | Dynamic motion capture and edition using a stack of tasks[END_REF] is a highly efficient whole body dynamics motion generation method with task prioritization. Erez et.al [START_REF] Erez | Trajectory optimization for domains with contacts using inverse dynamics[END_REF] generates running gaits based on inverse dynamics under external contacts. Since bipedal locomotion devices are meant to move around in unstructured environment, various approaches exist to clear obstacles with humanoid robots (e.g. ASIMO [START_REF] Michel | Vision-guided humanoid footstep planning for dynamic environments[END_REF] and HRP-2 [START_REF] Guan | Feasibility: Can humanoid robots overcome given obstacles[END_REF], [START_REF] Verrelst | Mobility of humanoid robots: Stepping over large obstacles dynamically[END_REF]). Guan et. al. [START_REF] Guan | Feasibility: Can humanoid robots overcome given obstacles[END_REF] conducted a kinematic feasibility analysis of obstacles to be overstepped concluding on a maximum obstacle of 24.21cm x 5cm (Width x Height). Dynamic overstepping was then achieved in [START_REF] Verrelst | Mobility of humanoid robots: Stepping over large obstacles dynamically[END_REF] as part of a walking pattern, in simulation and real experiment pushing the maximal values to 18cm x 11cm (Width x Height) including a safety margin of 3cm. The motion is planned for feet and hip (including twist) trajectories from a previous geometric feasibility analysis. The whole body motion is then generated based on the powerful ZMP preview control pattern generator from Kajita et.al. [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF]. Apart from the general on-line or off-line motion generation process a very difficult task is to stabilize the humanoid robot in real-time against external perturbations during the actual motion performance. In all robots from HRP (Humanoid Robotics Project) this on-line control component is called the stabilizer [START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF] and accomplishes most commonly a posture and a force control loop. The control component adjusts the reference joint angle trajectories such that, the desired posture reference coincides with the on-line estimation from the Kalman filter and the measured ground contact reactions follow the given ZMP reference. The force control is achieved by combination of the position controlled joints with a passive elasticity device [START_REF] Kajita | Balancing a humanoid robot using backdrive concerned torque control and direct angular momentum feedback[END_REF] and a force torque sensor forming a feedback loop that is adjusted for maximal backdriveability. The same device acts in parallel as low-pass filter to reduce shock effects from foot to ground impact [START_REF] Nakaoka | Constraint-based dynamics simulator for humanoid robots with shock absorbing mechnisms[END_REF]. From the viewpoint of motion performance the complete system mostly operates as if the elasticity would not be present [START_REF] Verrelst | Mobility of humanoid robots: Stepping over large obstacles dynamically[END_REF].

B. Contribution of this article

This article proposes a new approach based on whole body optimal control to investigate the potential limits for dynamically highly challenging motions. It is based on the common idea that, in case one would like to push the robot to its dynamic limits, the stress on the control system should by minimized. In consequence a candidate solution should be computed such that particularly the control action of the stabilizer is minimal. In this respect, the control system should be able to operate safely, despite perturbations, originating from model deficiencies and the external environment. The concept is applied to the dynamically, highly challenging case of overstepping obstacles with the robotic platform HRP-2, performing only two steps and with a static posture as initial and final boundary condition. The problem is formulated as multi-phase optimal control problem, based on the whole body dynamic model, including all limitations on joint angle ranges, velocity as well as the actuation systems. Furthermore abstract criteria such as dynamic stability and intensity of stabilizing control actions are considered. The solution is solely based on the governing physics -any other information such as phase timing and joint angle trajectories are computed during the process and do not need to be specified in advance. Finally the strategy is verified with motion experiments on the real robotic platform HRP-2 14 of LAAS-CNRS in Toulouse.

II. MODELING

A. Dynamic Modeling of the robot HRP-2

Despite the fact that the use of any robot with a sufficiently accurate model is possible, the chosen robotic platform for this analysis is the HRP-2 14. Technical information is given in [START_REF] Kaneko | Humanoid robot HRP-2[END_REF]. As it is essential to modeling in optimal control that simplifications are carefully chosen to preserve the governing physical characteristics at maximum -a detailed outline is given below: The robot HRP-2 [START_REF] Kaneko | Humanoid robot HRP-2[END_REF] is controlled by high gain position control driving brushed DC motors through gearboxes with a high reduction ratios. As the position control carefully rejects external perturbation the joint trajectory reference governs the motion characteristic, while the torque profile only gives an estimation about the mechanical energy characteristics. Thus dry and viscous friction representing an offset to the torque profile is not considered. The reduction ratios are sufficiently high to neglect dynamic coupling of the rotor inertia and the kinematic structure. Further decision was made to assume the gearbox as perfectly rigid to reduce mathematical complexity. In contrast to its predecessors [START_REF] Hirukawa | Humanoid robotics platforms developed in hrp[END_REF] the kinematic structure of the robot has been conceived for maximum stiffness [START_REF] Kaneko | Humanoid robot HRP-2[END_REF] and mass concentration in the pelvis (hosting the batteries). Consequently the kinematic structure is assumed perfectly rigid. The feet of the robot are rubber coated [START_REF] Nakaoka | Constraint-based dynamics simulator for humanoid robots with shock absorbing mechnisms[END_REF] and mounted Left Foot GCR ::Mx::

0 2 4 -100 0 100 200 t in [s] Force in [N]
Left Foot GCR ::Fx::

0 2 4 0 20 t in [s]
Torque in [Nm] Left Foot GCR ::My::

0 2 4 0 200 t in [s] Force in [N]
Left Foot GCR ::Fy::

0 2 4 -40 -20 0 t in [s]
Torque in [Nm] Left Foot GCR ::Mz::

MUSCOD OpenHRP HRP-2 0 2 4 0 200 400 600 Limit t in [s] Force in [N]
Left Foot GCR ::Fz:: Fig. 5. Comparison of the left foot ground contact reactions for optimization results (MUSCOD), the simulation of the virtual robot (OpenHRP), as well as during the experiment on HRP-2. Close trajectories are observable for the contact forces Fx,Fy, Fz (vertical force) and the Mz (vertical torque).

The stabilizing algorithms manipulate the components Mx, My as well as Fz. The vertical force limit should be kept firmly, however small peaks are not a problem.

with elastic bushings underneath a 6D force sensor to the ankle joint complex. While the elastics of the rubber coating are mostly negligible, the ankle elasticity mechanism [START_REF] Kajita | Balancing a humanoid robot using backdrive concerned torque control and direct angular momentum feedback[END_REF], [START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF] serves not only as shock absorber during bipedal locomotion. It is the physical component of the force torque control loop, built on top of the underlying joint position control loop to track the ZMP [START_REF] Vukobratovic | On the stability of anthromomorphic systems[END_REF] reference for dynamically stable operation. The employed two DOF torque controller [START_REF] Kajita | Balancing a humanoid robot using backdrive concerned torque control and direct angular momentum feedback[END_REF] is optimized for maximum backdriveability, hence distortion free frequency response for the reference signal is only guaranteed in low frequency space. Hence the elasticity is modeled as 2D rotational spring/damper system. The vertical elasticity effect will be neglected during this analysis for the sake of simplicity. The previously mentioned control system is part of a stabilizing algorithm that accomplishes the following tasks [START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF]: Control of the pelvis postures based on the estimation of the orientation from the IMU in the upper torso and heuristicsbased distribution of the vertical force on both feet as well as the contact torque. Torque and vertical force are directly related to the local CoP and the ZMP as long as the feet are firmly placed on the ground [START_REF] Vukobratovic | On the stability of anthromomorphic systems[END_REF]. The control reference is Left leg -Ankle Y

0 2 4 -0.2 -0.1 0 0.1 Security t in [s] Angle in [RAD] Right leg -Ankle X MUSCOD OpenHRP Ref-OpenHRP HRP-2 Ref-HRP-2 0 2 4 -0.4 -0.2 0 0.2 t in [s]
Left leg -Ankle X Fig. 6. Comparison of some joint trajectories for the lower torso part, for optimization results (MUSCOD), the simulation of the virtual robot (OpenHRP), as well as during the experiment on HRP-2. The control reference (scattered line) and the system's (solid line) are drawn separately for verification. The limit marks the absolute maximum angle range for the corresponding joint. The security limit was the actual limit chosen during the optimization to enforce safe operation. The dashed gray/red rectangle show parts of the trajectory where the stabilizer actively manipulated the motion to preserve dynamic stability. In red rectangles even the simulation and the real robot act substantially different.

obtained from the dynamics of the linear inverted pendulum [START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF]. In the former variant the angular momentum change of the body is not considered for control. The correction of the trajectory profile is distributed over the lower body by means of inverse kinematics while turning around the CoM of the robot [START_REF] Kajita | Zmp-based biped running control[END_REF]. As it is not clear which of the present stabilizer variants is currently implemented on HRP-2 14, the most probable and constraining option is considered for this analysis.

B. Stepping over motion

The generation process is formulated as multi-phase hybrid dynamic, optimal control problem with continuous, as well as discontinuous phase transitions (see figure 1, sequence 1-12 left-right/top-down):

The motion initiates from a static posture towards unloading the right foot (1-3, highlighted in grey). The right foot lifts off ground and travels over the obstacle (4-6). Following the As soon as the CoP reaches this boundary the contact stability is breached.

The mark limit represents the constraint that was used during optimization.

ground impact of the right foot, a short double support phase (highglighted in grey) is used to transfer the body support on the right leg and unload the left foot (6-7). Then the left foot lifts off ground and is retired over the obstacle [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF][START_REF] Kajita | Zmp-based biped running control[END_REF][START_REF] Kaneko | Humanoid robot HRP-2[END_REF]. After ground impact of the left foot, the robot enters a re-stabilization phase that ends in a static posture (10-12, highlighted in grey).

The employed problem formulation has the advantage that the motion characteristics (joint trajectories, phase timings, torque profiles) are determined during the optimization process, based on abstract high-level objectives and the governing physics. Phase transitions are determined by implicit switching constraints (a foot may lift off ground as soon as the contact reactions vanish) and discontinuous phase transitions are assumed to be perfectly inelastic (a foot that reaches ground level collides).

C. Equations of Motion

The robot is modeled in minimal coordinates (without contacts) with 30 dof for its internal branched tree structure, 4 dof to account for its elasticity in the ankle, as well as further 6 dof to model the free motion in the global reference. This coordinate configuration is preserved throughout the complete phase cycle featuring position contacts and kinematic loops. The resulting dynamic equation is then expressed as DAE system of index 3 formulated in the socalled descriptor form.

M (q, p) q + N LE (q, q, p) +C (q, p) -J (q, p)

T λ = τ (1)

g (q, p) = 0 (2)
In this equation M (q) represents the joint space inertia matrix which consists of the inertia matrix from the kinematic tree structure of the robot

M = k J T k I k J k + M m (3)
and an additional diagonal term M m (4) containing the simplified dynamic effects of the spinning coils in the joint actuators (where I m i is the rotational inertia of the coil i about its spinning axis and R i the ratio of transmission of the joint i):

M mii = R 2 i I m i i < N 0 i ≥ N (4)
N LE (q, q) represents the nonlinear effects (e.g. Coriolis). Furthermore we include here the dynamic effects of the spring and damper systems. The following analysis is based on parameters roughly chosen by hand.

N LE = k J T k I k Jk q -qT J T k × I k J k q +K D q + K p q (5)
J k represent the spatial Jacobian to the local reference frame of the link k respectively. I k is the spatial inertia matrix of link k. K D and K P are matrices that hold spring and damping constants with respect to each of the degrees of freedom [START_REF] Nakaoka | Constraint-based dynamics simulator for humanoid robots with shock absorbing mechnisms[END_REF] Gravity effects are considered with the term C (q). τ represents the system's actuation as torque on the joint-level (rotational). The term g (q) expresses the scleronome position constraints.

For higher computational efficiency, this is transformed into a DAE system of index 1:

q = v (6) v = a (7) M -J T J 0 a λ = -N LE -C + τ -γ (8)
In forward dynamics algebraic variables λ (equivalent to the contact constraint forces) and acceleration q values are computed from given system's state q, τ . Additionally, the following conditions arising from index reduction have to be respected for consistency:

g (q (0)) = 0 (9) J (q (0)) q (0) = 0 [START_REF] Leineweber | An efficient multiple shooting based reduced SQP strategy for large-scale dynamic process optimization -Part I: theoretical aspects[END_REF] The term J is the classical (not the spatial!) analytical 6D Jacobian and γ = ∂ ∂t (J) q its time derivative multiplied by the velocity vector respectively that contains all active constraint directions in the contact points.

When a foot enters in contact with the ground, this is modeled as an inelastic impact causing discontinuities in the joint velocities which can be computed by the following equation. qand q+ represent the velocity configuration before and after the impact respectively and Λ holds the complex impact impulsion:

M -J T J 0 q+ Λ = M q- 0 (11)
The dynamic equations are composed analytically, optimized and converted into C-code by means of our own dynamic model builder DYNAMOD based on 6D spatial geometry [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. Inspired by the the work of Wieber [START_REF] Wieber | The humans toolbox, a homogenous framework for motion capture, analysis and simulation[END_REF], the code generation is done building on top of the commercially available package Maple TM .

III. GENERATION OF OPTIMAL STEPPING MOTIONS BY OPTIMAL CONTROL TECHNIQUES

In the following the implementation of abstract motion characteristics in the problem formulation will be intensively exploited to generate motions that will rely on the whole body dynamics but at the same time minimize the control effort of the stabilizing algorithm.

A. The optimal control problem

In order to generate the stepping motion over the obstacle, a multiple-phase optimal control problem is formulated (similar to [START_REF] Schultz | Modeling and optimal control of human-like running[END_REF]). An optimal control problem is an optimization problem where the unknown variables are functions in time and dynamic equations are respected as constraints. The general formulation [START_REF] Wieber | The humans toolbox, a homogenous framework for motion capture, analysis and simulation[END_REF][START_REF] Michel | Vision-guided humanoid footstep planning for dynamic environments[END_REF][START_REF] Roussel | Generation of energy optimal complete gait cycles for biped robots[END_REF][START_REF] Schultz | Modeling and optimal control of human-like running[END_REF][START_REF] Kajita | Balancing a humanoid robot using backdrive concerned torque control and direct angular momentum feedback[END_REF][START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF][START_REF] Nakaoka | Constraint-based dynamics simulator for humanoid robots with shock absorbing mechnisms[END_REF][START_REF] Erez | Trajectory optimization for domains with contacts using inverse dynamics[END_REF] of this problem uses the time t, the system states x(t), the controls u(t) and system parameters p. Φ i (x(t), u(t), p) dt + Ψ i ( ti , x ( ti ) , p) [START_REF] Wieber | The humans toolbox, a homogenous framework for motion capture, analysis and simulation[END_REF] subject to ẋ(t)f i (t, x(t), u(t), p) = 0 (13)

x t+ i -h i x t- i = 0 (14)
r eq x(0), T, x(T ), t0 , x t0 , ..., ts , x ts , p = 0 (15) r ineq x(0), T, x(T ), t0 , x t0 , ..., ts , x ts , p ≥ 0 (16)

g i (x(t), u(t), p) ≥ 0 (17) u ≤ u ≤ u (18) x ≤ x ≤ x (19)
The objective function ( 12) consists of a continuous Lagrange-term (Φ) and an end time related Meyer-term (Ψ) for each phase. Optimization is done with respect to the system dynamics (13), equality and inequality boundary constraints [START_REF] Schultz | Modeling and optimal control of human-like running[END_REF][START_REF] Kajita | Balancing a humanoid robot using backdrive concerned torque control and direct angular momentum feedback[END_REF] for all phases, continuous path constraints [START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF] as well as box-constraints (limits) on system states [START_REF] Erez | Trajectory optimization for domains with contacts using inverse dynamics[END_REF] and controls [START_REF] Nakaoka | Constraint-based dynamics simulator for humanoid robots with shock absorbing mechnisms[END_REF]. i ∈ M, M = {0...r} regroups all indices of the discrete phases. t+ i and ti denote the specific instant of phase transition, the former before and the latter after the phase transition. Without loss of generality it is assumed that ts = 0. f i is the phase dependent right hand side of a first order ODE. Specifically in this problem formulation the state vector of the system x = [q, q, τ ]

T ∈ R 110 comprises joint positions q ∈ R 40 and velocities q ∈ R 40 (reduction to first order ODE) and the joint torques τ ∈ R 30 . The controls u ∈ R 30 are injected into the system as torque derivatives τ to assure continuity of joint torque trajectories during phase transitions. In previous studies this characteristic improved as well the quality of the approximated system input. h i expresses characteristics of the phase transition (i-1) → (i) (e.g. inelastic impact). f i , r eq , r ineq , g i are vector functions with correspondent dimension. Besides the desired final motion characteristics a decision whether a characteristic must be enforced as constrained or objective must be carefully made.

B. Objectives

The objectives are divided into essential (dominant) and overall (sub-dominant) motion characteristics achieved, by individual weights ω name in the objective function.

1) Minimal excitement of ankle elasticity: From the previous presentation (see I-A and II) it is clear that the quality of the backdriveability of the controller is only preserved in the low frequency space, hence allowing only small values for the acceleration and velocity of the corresponding degrees of freedom. As the incorporated elasticity model is only a linear approximation it is only expected to be valid in a small region around the neutral working point:

min Φ Elasticity = ω Acc i∈Elast qi 2 +ω V el i∈Elast qi 2 + ω P os i∈Elast q 2 i (20)

2) Minimal linear vertical momentum of CoM:

The quality of the simplified modeling of the elasticity may be further improved when excessive oscillation of the vertical force on the elasticity is prevented, as these effects are assumed to be substantially correlated. Furthermore this assumption minimizes the height variations of the CoM and thus deviation from the linear inverted pendulum mode [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF] a central assumption of the stabilizer [START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF].

min Φ Linear Momentum = ω LinM omZ (L z CoM ) 2 +ω ∂LinM omZ ∂ ∂t L z CoM 2 (21)

3) Minimal vertical angular momentum change about CoM & vertical contact shear torque:

As a motion to dynamically overstep an obstacle may drive the foot contact during single support to its adhesive limits, this objective combination helps at a time to minimize the necessary vertical torque to be canceled in the foot contact and at the same time excessive motions with the upper torso.

min Φ Angular Momentum = ω ∂AngM omZ ∂ ∂t H z CoM 2 ( 22
)
min Φ Contact Torque = ω ContactZ f mZ Contact foot 2 (23)

4) Minimal deviation between whole body ZMP and table cart ZMP:

The difference between the whole body ZMP [START_REF] Vukobratovic | On the stability of anthromomorphic systems[END_REF] and the 3D-LIPM [START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF], as it seems to be applied in the stabilization algorithm, is the effect of the change of the horizontal components of the angular momentum about the CoM. These could be used to cancel out the acceleration effect of the CoM about the ground. In the reverse conclusion, it is possible that the optimization tends to exploit this effect to improve the dynamics of the motion, but hence fails to comply with the assumption of the stabilizer. This would risk in a highly unstable motion, a situation that should be avoided if possible. Consequently the euclidean norm of the geometric difference is minimized. min Φ ZMP -3D-LIPM = ZM P x,y whole body -ZM P x,y (24)

5) Minimal performance time: As motion capacities of humanoid robots are still far behind those of humans, it will be interesting to investigate how fast the robot platform HRP-2 is capable to clear the obstacle (in simulation of course) within its strict kinematic and dynamic limits:

min Ψ time = T Global End-time (25)
6) Head stabilization: In [START_REF] Pozzo | Head and trunk movements in the frontal plane during complex dynamc equilibrium tasks in humans[END_REF] it was reported that angular stabilization of the head and gaze is essential for a dynamic postural control during various manipulation and locomotion tasks. At one hand the desired motion represents a bipedal locomotion problem and at the other hand a tedious postural balance problem. Apart from the fact that HRP-2's IMU is not located in the head, but in the chest [START_REF] Kaneko | Humanoid robot HRP-2[END_REF], is concluded that head stabilization, towards the final target in the global reference frame, is an essential characteristic to improve postural stability as well as the general appearance. min Φ Head = (an) Yaw (x) 2 + ((an) Pitch (x)p Pitch-Offset ) 2 +(an) Roll (x) 2 (26) 7) Minimal variations of squared torques: As stated in [START_REF] Schultz | Modeling and optimal control of human-like running[END_REF] it is possible to improve the motion-quality further by minimizing sub-dominantly the squared first derivative of the joint torques. This criteria was mostly integrated for technical reasons (huge control oscillations):

min Φ torque = 30 j=1 (u j ) 2 (27)

C. Path constraints

For computational efficiency it is crucial to keep mathematical complexity at the lowest possible level. Besides neglected physical effects, one needs to employ various continuous path constraints to prevent unrealistic physical behavior of the system during the simulation.

1) Foot contact/clearance: As the foot to ground contact is uni-lateral, a valid contact state necessitates pressure in the contact surface. Identification of this condition is done based on the vertical contact constraint force and the center of pressure (henceforth called CoP) bound to a subspace of the contact surface [START_REF] Ramos | Dynamic motion capture and edition using a stack of tasks[END_REF]. For this analysis the feasible area is reduced to square of 1[cm] x 1[cm] as we will explain in the result section. Slipping was not explicitly modeled in the foot contact and hence an adequate contact state needs to be enforced. In a first approximation the vertical contact force component is used to establish a maximal admissible horizontal force component -Coulomb friction cone -and a maximal admissible vertical torque component. Friction effects are not considered during swing (foot to ground). Consequently a foot clearance is enforced based on a time dependent smooth curve of minimal height of the lowest point of the foot fold to ground distance.

2) Self Collision: An important issue in humanoid robot motion generation is self-collision. As joints are position controlled, unexpected contact during a performed motion usually leads to mechanical deterioration of either the kinematic structure or the concerned actuation system. From the complex kinematic structure the collision free workspace is highly scattered and even during simple dynamic motions, like reaching or walking, self collision between various links is very likely to occur. Additionally possible collisions with the given obstacle, the robot has to step over, need to be avoided. For collision detection a simple line geometry approach based on cylinders [START_REF] Ketchel | Collision detection of cylindrical rigid bodies using line geometry[END_REF] but with rounded caps is employed.

3) Pelvis stabilization: In preliminary trials the optimal control problems relayed mainly on the upper torso dynamics to stabilize the whole body motion. As firstly the quality of the dynamic model in the upper torso section is considerably doubted and secondly the effect of a highly dynamic reorientation of the pelvis section on the stabilizing algorithms is not yet clear, the decision was made to keep roll and pitch orientation in tight angular ranges (-0.005, +0.005) [RAD].

D. Solving optimal control problems

The proposed optimal control problem formulation is solved with the powerful framework MUSCOD II, developed at the University of Heidelberg. Based on early works of Bock and Plitt [START_REF] Bock | A multiple shooting algorithm for direct solution of optimal control problems[END_REF], it has been implemented by Leineweber [START_REF] Leineweber | An efficient multiple shooting based reduced SQP strategy for large-scale dynamic process optimization -Part I: theoretical aspects[END_REF]. The core algorithm consists of a direct multiple shooting method. First a discretization of the system controls into a finite set of additional system parameters is performed, based on an approximation with parametric model functions (piecewise constant/linear, splines) on a given time grid. The choice of the time grid determines the quality governing the approximation of the controls. The system trajectory is further parametrized as a series of initial value problems with additional matching conditions to assure continuity across the multiple shooting intervals. Values for the objective function, boundary and path constraints, including all sensitivities of the trajectory with respect to initial conditions and parameters are then computed through internal numerical differentiation (IND [START_REF] Leineweber | An efficient multiple shooting based reduced SQP strategy for large-scale dynamic process optimization -Part I: theoretical aspects[END_REF]) to allow for high accuracy. Upon this information a large but highly structured general nonlinear program is formulated. This NLP is then solved based on a sequence of equivalently condensed QP sub-problems [START_REF] Leineweber | An efficient multiple shooting based reduced SQP strategy for large-scale dynamic process optimization -Part I: theoretical aspects[END_REF], where additional parameters of the system trajectory are recursively eliminated. In our set-up, time grids for control and state trajectory discretisation are the same. Optimization was carried out in a three subsequent steps.

First the simulation setup was employed to compute a two step walking motion from static half-sitting posture. As soon as a feasible walking trajectory was found after a few iterations, the obstacle was included in the setup to optimize a stepping motion over the obstacle until a certain obstacle height was reached. Finally the motion was further optimized to form the desired trajectory characteristics.

IV. RESULTS

As the interest of this analysis is not to investigate the maximum obstacle height, but to assess the quality of this approach, optimization has not been tuned towards a maximum obstacle height, but a suitably smooth motion combined with a reasonably obstacle height for safe investigation during real experiments. Thus we chose an obstacle height 20cm (height) x 11cm (width) including safety margin. 2). The simulation of the virtual robot and the real experiment showed a smooth motion without slipping, medium ground impact collisions and no destabilizing. The analysis of the objectives revealed that the optimization converged mostly with desired motion characteristics as dominant objectives: minimum excitement of the ankle elasticity in horizontal and vertical direction, minimal difference between real ZMP and table cart ZMP. From figure 5 it is clearly observable that the whole body model in our optimization framework, the virtual robot as well as the real robot show relative close motion characteristics. The force/torque control loop only concerns the ground contact reactions Mx, My and Fz. Apart from noise and different initial conditions in the optimization and the simulator OpenHRP the ground contact reactions Fx, Fy, Mz and even Fz closely follow the computed results. Thus it is concluded that perturbation from vertical excitement of the elasticity has been canceled out sufficiently. Contrary Mx and My show large deviation and consequently some chosen motion characteristics of the computed reference need still improvement. This is further confirmed from figure 6. Deviation from the computed reference during the real experiment and even the simulation are clearly observable in the ankle joint as well as in the knee joint of both legs. This verifies as well the aspect that the whole lower body is used to apply the manipulation of the trajectories of the stabilizer. Besides these deviations, the overall trajectories coincide relatively well most of the time. Furthermore the deviation of the posture control, visible in the global orientation of the torso stays small (see figure 4). Thus we conclude, that our employed strategy was successful, despite, modeling errors (elasticity, mass distribution) and the reference trajectory, that seem not to be fully compatible with the stabilizing algorithms, but hence has even to account for potential differences between simulation and real robot (see figure 6 red mark). A comparison between figure 3 and 7 reveals an interesting fact. Figure 3 shows deviations between the real ZMP and the Table Cart ZMP (as this objective was not triggered to be the first dominant one -these deviations are reasonable). Apart from the high peaks during the phase transitions (single support ↔ double support) a high correlation between the deviations in figure 3 and7. Same tendency is visible on figure 5 (see plot My !). Even though the CoP was constrained to move only in a squared area of 1 [cm] x 1 [cm] (blue line), the final result of the stabilizer moves the CoM in a much larger area of approximately 10 [cm] x 10 [cm], which is however still compatible with the geometry of the foot (see figure 7). Besides simplifications in the modeling of the elasticity, this might give evidence about the stabilizing algorithm following the table cart ZMP reference [START_REF] Kajita | A running controller of humanoid biped hrp-2lr[END_REF]. The local CoP in the left foot -especially during single support -moves to the opposite direction of the observable deviation between the real and the table cart ZMP. Thus the stabilizing algorithm seems to not account for the dynamic compensation based on the angular momentum and consequently deviates from the computed reference.

V. CONCLUSIONS & PERSPECTIVE

This paper proposes a new generic strategy to investigate the hardware and control limits of a given robotic platform based on a dynamically challenging motion. This strategy massively exploits the possibility to formulate desired abstract motion characteristics based on high-level objectives. Despite unavoidable modeling errors and small incompatibility of our computed reference trajectories to the stabilizing algorithms, the real robotic platform was still able to safely operate during the motion performance. Based on the previous analysis further adjustments to the formulation of desired motion characteristics should lead to a more predictable behavior of the motion control and stabilizing algorithms, to investigate the actual li¡mits even more precisely. Future prospects will be further refinements to the whole body dynamics model as well as extension to other dynamically challenging motions, such as dynamically stepping through a narrow passage.

  Optimisation in Robotics and Biomechanics, IWR University of Heidelberg, INF 368, 69120 Heidelberg, Germany 3 ostasse at laas.fr 4 psoueres at laas.frGEPETTO, LAAS-CRNS TOULOUSE, avenue du Colonel Roche 7, 31031 Toulouse cedex 4, France

Fig. 1 .

 1 Fig. 1. image sequence of the stepping motion

Fig. 7 .

 7 Fig.7. Plot of the trajectories of the local center of pressure of each foot in X and Y for optimization results (MUSCOD), the simulation of the virtual robot (OpenHRP), as well as during the experiment on HRP-2. High peaks occur during the transition phase caused by the vertical contact force dropping to 0. The mark foot represents the physical boundary of the foot. As soon as the CoP reaches this boundary the contact stability is breached. The mark limit represents the constraint that was used during optimization.

Table Cart 2 2

 Cart2

  The obstacle is successfully cleared in 4.32[s] (result of optimization), in 4.33 [s] and 4.34[s] with the virtual robot in simulation and the real robot platform respectively. The optimization freely chooses a step-length of 0.415 [m] and a relatively narrow foot arrangement of 0.17 [m] between feet to clear the obstacle. The robot starts from a nearly symmetric posture and a narrow feet arrangement (0.16 [m] lateral offset, 0.005 [m] sagittal offset) into a slightly larger arrangement to re-stabilize the robot (0.19 [m] lateral offset, 0.09 [m] sagittal offset) into the final static posture (see figure 1 and figure

ACKNOWLEDGMENT

Financial support of HGS MathComp and the KOROIBOT Project for this work is gratefully acknowledged. We thank the SimOpt team (Hans Georg Bock) of the University of Heidelberg for providing the optimal control code MUS-COD. The authors would like to thank as well Maximilien Naveau for his valuable help during the experiment-phase on the HRP-2 robot.


Paul Evrard
email: evrard.paul@aist.go.jp

Nicolas Mansard
email: nmansard@laas.fr

Olivier Stasse
email: olivier.stasse@aist.go.jp

Abderrahmane Kheddar
email: kheddar@ieee.org

Thomas Schauß
email: schauss@tum.de

Carolina Weber
email: carolina.weber@tum.de

Angelika Peer
email: angelika.peer@tum.de

Martin Buss

Intercontinental, Multimodal, Wide-Range Tele-Cooperation Using a Humanoid Robot

This paper is the continuation of our previous work in intercontinental, collaborative teleoperation with a humanoid robot. Our new achievement consists in an extension of the former single-arm bilateral teleoperation setting to include bimanual manipulation and walking. A coupling scheme for simultaneous manipulation and locomotion is developed. Furthermore, a task-based control framework, including a force-based control for the arms as well as a walking pattern generation, is presented to realize stable whole-body motions of the highly redundant humanoid robot. Experiments have been performed to assess the proposed control scheme. They bring to light additional scientific challenges that remain in order to reach a smooth and natural telepresent collaboration.

I. INTRODUCTION

Teleoperation consists in remotely controlling a robotic system, the teleoperator, to perform tasks at a distance. In doing so, a multi-modal human-system interface provides sensory feedback to the human operator and allows him/her to interact with the remote environment by mapping her/his actions to the teleoperator.

Teleoperating fixed based robotic arms is a well investigated problem in robotics; even mobile robots have been operated at a distance by means of joysticks [START_REF] Lee | Bilateral Teleoperation of a Wheeled Mobile Robot Over Delayed Communication Network[END_REF] or foot pedals [START_REF] Caldwell | Telepresence Feedback and Input Systems for a Twin Armed Mobile Robot[END_REF] or by using a mobile haptic interface [START_REF] Nitzsche | A Mobile Haptic Interface Mastering a Mobile Teleoperator[END_REF]. In this paper, we are interested in realizing a teleoperation system that allows for remotely controlling a humanoid which is performing a joint physical collaborative task with another, onsite, human. So far, humanoid robots have been teleoperated using supervisory control to generate whole body motion and bilateral coupling to control the hand and foot positions [START_REF] Yokoi | A Tele-Operated Humanoid Operator[END_REF], or using advanced virtual reality technology [START_REF] Kheddar | Enhanced Teleoperation Through Virtual Reality Techniques[END_REF].

As this paper presents the continuation of our previous work [START_REF] Peer | Intercontinental Multimodal Tele-Cooperation Using a Humanoid Robot[END_REF], we focus hereafter on the problems that are brought to light when the collaborative teleoperation task is conducted while walking, extending the workspace to a much wider range. We consider an operator using a head mounted display (HMD) and a bimanual haptic interface mounted on This work is in part supported by the EC FP6 project ROBOT@CWE www.robot-at-cwe.eu and the German Research Foundation (DFG) within the collaborative research center SFB453 "High-Fidelity Telepresence and Teleaction". For the content of this paper the authors are solely responsible for, it does not necessarily represent the opinion of the European Community. an omnidirectional, nonholonomic mobile base (MHI) as a master system coupled to a humanoid teleoperator (TOP) as slave system, see Fig. 1.

Various bilateral coupling schemes can be thought of when coupling these two devices. For instance, one can consider the mobile platform at master side and the humanoid at teleoperator side as independent 'tools', which interact with the human operator and the manipulated object. To avoid discrepancies between the postures of the operator and the teleoperated robot, which might decrease the feeling of presence and task performance, a posture reflecting scheme can be implemented that maps the tracked posture of the operator and best-copies it on the distant humanoid platform.

In this paper, we describe the lighter implementation where the posture of the operator is not tracked. Our contribution regarding the system described in [START_REF] Peer | Intercontinental Multimodal Tele-Cooperation Using a Humanoid Robot[END_REF] is the possibility to perform wide area motions by moving the mobile platform and thereby making the humanoid platform walk. The main difficulty we faced is that the types of motion for each system are different: the master platform moves through a continuous contact interaction (wheels) while the humanoid robot's motion is made through discrete alternate contacts. The impact of this different characteristics of motions is highlighted in the experimental results.

We report hereafter on the goals achieved so far in the realization of a collaborative teleoperation task while walking. We further discuss what remains to be improved to reach a highly sophisticated, smooth, and natural teleoperation.

II. COUPLING SCHEME

When integrating locomotion into the existing teleoperation system, various coupling schemes are possible to position the mobile platform and the humanoid robot. A possible control design, shown in Fig. 2, is to only reflect the interaction between the TOP and its environment. This can be achieved by classical bilateral coupling between the end-effectors of the TOP and the MHI. The pose of the TOP and of the MHI are controlled independently to allow widerange motion of the end-effectors. On master site, the MHI is positioned using a local manipulability-optimizing controller, and at teleoperator site the TOP is controlled to generate a stable gait so as to keep the end-effectors at a given position with respect to the body of the robot. This scheme allows an almost complete separation of the constraints related to each platform (e.g. dynamic balance of the walking humanoid robot), since only the end-effectors are coupled. However, as the postures of the platform are controlled independently, discrepancies between the attitude and positioning of each platform relative to the local user will certainly occur. This is not critical at the master site, since the operator is totally immersed, but it can be problematic at the teleoperator site. If the humanoid posture and the operator posture are dissimilar, the remote operator might misunderstand the intentions of his/her partner. Moreover, it is important for the master operator to have visual feedback that conforms to his/her actual posture in order to take appropriate actions via the teleoperator platform. Moreover, as the locomotion of the distant humanoid robot is decoupled from the locomotion of the operator, the feeling of immersion is impaired. Finally, since the locomotion of the TOP results from the motion of its end-effectors, it is necessary to apply forces to reposition the teleoperator.

A possible solution for these problems, illustrated in Fig. 3, is to add a posture reflecting scheme on top of the task-space coupling scheme. In doing so, the humanoid robot would use its redundancy to best-copy the posture and relative position of the master operator under platformspecific and environmental constraints such as dynamic balance and self collision avoidance. The MHI at the master site is still controlled independently, since the operator does not perceive its posture. Such a scheme would probably result in a better understanding between the partners since information about the operator's intentions would be conveyed through the humanoid posture. It also allows decoupling of locomotion and manipulation, and would result in a better feeling of presence, especially due to congruence between visual and proprioceptive feedback. This scheme requires the posture of the operator to be tracked and raises challenges, since the gait and balance of the robot must be generated while mimicking the posture of the operator. For the time being, this last solution has not been implemented. The remainder of this paper will describe a lighter implementation where only the end-effectors are coupled. It requires fewer communication channels, needs no tracking of the operator's position and incorporates locomotion into the existing system presented in [START_REF] Peer | Intercontinental Multimodal Tele-Cooperation Using a Humanoid Robot[END_REF] by only modifying the local control loops. An evaluation of this scheme will determine how the feeling of presence is impaired when the posture of the humanoid robot and the operator do not match and whether enhancing this scheme with a posture reflecting coupling is necessary to achieve a desired degree of immersion and task performance.

III. LOCAL CONTROL ON SLAVE SIDE

On the remote side, a HRP-2 humanoid robot with 30 actuated degrees of freedom (DOF) is used to mirror the actions of the master operator. It is capable of bipedal locomotion, and is equipped with force sensors at the wrists and at the ankles. To take advantage of the high redundancy provided by this platform, we have chosen to use a taskbased control approach [START_REF] Samson | Robot Control: the Task Function Approach[END_REF], [START_REF] Khatib | A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation[END_REF]. Such approaches simplify the definition and the implementation of a robotic behavior, especially in the case of highly redundant robots like humanoids. The approach provides whole-body control and can directly comprehend a walking pattern generator.

In the next paragraph, we will shortly recall the overall task-based control architecture. As the control is based on the kinematic inverse, care has to be taken while accounting for forces due to manipulation. A complete solution to obtain a transparent compliance of the arms of the robot is thus proposed in Section III-B. Finally, we recall the algorithm used for robot walking, and present the scheme used to link the whole-body behavior to the step decision (Section III-C).

A. Stack of Tasks

A task is defined by the triplet e, J, ė * , where e defines the space of the measure (typically, e could be an error between a sensor measure s and its reference value s * : e = ss * ), J = ∂e ∂q is the Jacobian of the task, and ė * is the vector field defining the desired behavior of the system in the task space (typically, if requiring an exponential decrease of the error, the vector field is defined to be ė * = -λe, with λ the gain that tunes the robot velocity). Using these definitions, the model of evolution of the task is given by

ė = J q (1)
where q, the joint velocity of the robot, is considered as the input of the system. To obtain the reference ė, the generic solution is given by

q = J # ė * + Pz (2)
where J # is a Pseudoinverse of J [START_REF] Ben-Israel | Generalized Inverses: Theory and Applications[END_REF], z is any arbitrarily chosen vector and P is the projector onto the null space of J that guarantees that, for every z, the main task ė * is not modified. The vector z can then be used to realize a secondary task, and, by recurrence, any set of tasks hierarchically organized. The stack of tasks (SoT) is a structure that orders the tasks that are currently active. Only the tasks in the stack are taken into account in the control law. The task at the bottom level has priority over all the others, and the priority decreases as the stack level increases. Any new task added in the stack does not disturb the tasks already in the stack. The control law is computed using the redundancy formalism introduced in [START_REF] Siciliano | A General Framework for Managing Multiple Tasks in Highly Redundant robotic Systems[END_REF]. Let (e 1 , J 1 ) ... (e n , J n ) be n tasks. The control law computed from these n tasks ensures that the task e i does not disturb the task e j if i > j. A recursive computation of the joint velocity is proposed in [START_REF] Siciliano | A General Framework for Managing Multiple Tasks in Highly Redundant robotic Systems[END_REF]:

qi = qi-1 + (J i P A i-1 ) # ( ėi -J i qi-1 ), i = 1..n (3)
where P A i is the projector onto the null-space of the augmented Jacobian J A i = (J 1 , . . . J i ), J i P A i-1 is the limited Jacobian of the task i and q0 = 0. The robot joint velocity realizing all the tasks in the stack is q = qn . A complete description of the Stack of Tasks is given in [START_REF] Mansard | Task Sequencing for Sensor-Based Control[END_REF]. The Stack of Tasks is also extensible for humanoid (free-floating underactuated) robots [START_REF] Mansard | Visually-Guided Grasping While Walking[END_REF].

B. Force-Based Control

Using the foregoing control scheme, it is straightforward to realize many tasks in free space. However, for forcebased control and tasks involving contact, the redundancy formalism does not directly apply. A solution to this problem is the dynamic inverse as proposed in [START_REF] Khatib | A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation[END_REF], [START_REF] Park | Contact Consistent Control Framework for Humanoid Robots[END_REF] (rather than using the kinematic inverse (2)). However, this solution requires the robot to be torque controlled, which is not the case for our robot. We rather propose to modify the previous control scheme in order to obtain a similar behavior.

The space in which the control is designed is the operational space (6D position) of the contact point denoted by r. A common scheme for position-controlled robots is an admittance controller, that defines the reference through the differential equation of a virtual mass-damping system:

Mr + Bṙ = f ( 4
)
where r is a reference position and orientation in the task space, M and B are arbitrary mass and damping matrices of the equivalent virtual system, and f are forces and torques exerted on the equivalent virtual point. For this setup, f is defined as the sum of the real forces measured at the contact point and the reference forces sent by the master operator. In (2), the matrix J # can be chosen as any Pseudoinverse. We now choose to apply a weighted Pseudoinverse [START_REF] Doty | A Theory of Generalized Inverses Applied to Robotics[END_REF], defined when J is full-row-rank by

J #W = W -1 J T JW -1 J T -1 (5)
with W an arbitrary invertible matrix that represents the weights on the joints. Taking the derivative of ( 2) with z = 0 and introducing (5) yields:

q = W -1 J T JW -1 J T -1 M -1 (f -Bṙ) (6)
Selecting the weight W as the inertia matrix A of the robot and the virtual mass M as Λ = JA -1 J T -1 , the apparent inertia of the end-effector of the robot, we obtain:

Aq = J T f -B q q (7)
where B q = J T BJ is the friction factor of the whole body structure. This last equation corresponds to a simplified version of the dynamic equation of the whole-body compliant robot with gravity compensation, with forces f acting on r and a friction B q that may be tuned by selecting B to stabilize the system. Consequently, if the control parameters are chosen as described above, the obtained control is equivalent to the real dynamics of the robot. The control represents a generalization of (3), which means that both force-based contact tasks and position-based free space tasks can be realized within this control structure. No specific values have to be chosen or tuned, except for the damping gain B, that has been experimentally verified to be very robust.

This solution generalizes the use of the kinematics inverse for collaborative task. We now recall quickly how to incorporate the walking pattern generation into the overall framework.

C. Pattern Generator

Given the position of the next footprints, the pattern generator computes the robot's Center of Mass (CoM) and foot trajectories that ensure that the robot will not fall. The stability criterion used for controlling the robot not to fall is the zero moment point (ZMP), as the robot is currently walking on a flat floor.

The pattern generator used in the context of this experiment is based on a simplified model and the preview control method proposed in [START_REF] Kajita | Biped Walking Pattern Generation by Using Preview Control of Zero-Moment Point[END_REF]. The particularity of the algorithm is two-folded: it uses a preview window to compute the control law from a simplified Linearized Inverted Pendulum Model (LIPM), and then compensates for the difference between the LIPM and the multi-body model of the robot. The core part of this algorithm is to solve a quadratic programming (QP) problem, where the cost function is to minimize the jerk of the LIPM and to follow an ideal ZMP trajectory. From this ZMP trajectory defined by the foot prints given as input, the solution of the QP gives a CoM trajectory which is dynamically stable.

An appealing solution to apply this algorithm for walking gait generation on the humanoid platform would be to recompute the CoM and foot trajectories for the whole preview window at each control cycle. However, the computation cost to solve the QP problem with the multibody model is too high for the control loop (5ms) on the teleoperator. Hence the pattern generator is used as a planner which generates a stable motion within a fixed time (currently 3.2s). Consequently, it is impossible to input any footprint reference within this time window.

In practice, we used the pattern generator software described in [START_REF] Stasse | Modular Architecture for Humanoid Walking Pattern Prototyping And Experiments[END_REF]. To account for the preview window, four footprints had to be provided to the algorithm. A new footprint is then added each time one step has been executed by the robot. The output of the pattern generator are reference trajectories for the feet and for the CoM, that can be taken into account as top-priority tasks in the SoT.

D. Dynamic Balance and Collaborative Tasks

The challenge when controlling a walking humanoid platform to perform a physical collaboration task lies in the fact that interaction forces induced by the task create sudden perturbations on the ZMP of the robot. In order to keep dynamic balance, the footprints would have to be modified within the preview window. The technical difficulty to cope with this, is hence to be able to compute a new reference for the CoM and the feet trajectories with modified inputs in the preview window, and/or with a new initial state.

In this work, we have experimentally evaluated whether the humanoid platform could keep its balance while performing a collaborative task. It appears that the commercial stabilizer provided with the control software and the admittance control implemented for the end-effectors limit to some extent the interaction forces between the teleoperator and the onsite operator. Thus, perturbations on the ZMP of the robot are small enough for the stability criterion to be satisfied without changing the footprints in the preview window.

To decide on the next step to be provided to the pattern generator, we considered the distance of the current position to a reference position of the hands given in the waist coordinate system. Steps are planned to regulate this error to zero, such as to keep the end-effectors of the robot close to a suitable position in its workspace.

E. Definition of the Tasks

To summarize, seven tasks have been considered in the experiments. 1) e walk tracks the desired trajectories of the legs provided by the pattern generator. Since this task controls the absolute position and orientation of both feet of the robot it uses 12 DOF of the robot. In future work, only the position of the free flying foot will be controlled, and the support foot will be considered as fully constrained by the contact with the ground. This will relax 6 DOF of the robot, thus allowing a wider range of motions.

2) e com regulates the position of the CoM on the trajectory given by the pattern generator. 3+4) e zr and e zl regulates the altitude of both right and left hands of the robot to a desired fixed height, in order to simplify the task of the master operator when the robot is walking. Position of the end-effectors r r and r l is controlled by taking into account only the z component of the vector. These tasks are removed when the robot is not walking. 5) e fr and e fl are the force-based control of both right and left hands of the robot. They are defined as in [START_REF] Yokoi | A Tele-Operated Humanoid Operator[END_REF]. The reference force f is then the sum of real measured forces and reference forces sent by the master. 6) e head controls the orientation of the head according to the reference position given by the master operator. 7) e grip is finally added to control specifically the gripper aperture to the reference transmitted from the master.

IV. LOCAL CONTROL ON MASTER SIDE

As haptic interfaces two ViSHaRD7 are used on operator site [START_REF] Peer | A New Admittance-Type Haptic Interface for Bimanual Manipulations[END_REF]. The two 7-DOF arms have a human-sized workspace and high force output capabilities. The haptic interfaces can hereby perform movements in a workspace similar to that of the human operator at a fast speed. The interfaces were especially designed to be mounted on an omnidirectional, nonholonomic mobile base [START_REF] Unterhinninghofen | Control of a Mobile Haptic Interface[END_REF]. The mobile base can perform arbitrarily large movements at a slower speed. The combination is capable of displaying fast movements and high forces in an arbitrarily large workspace.

The haptic interfaces are controlled using position-based admittance controllers. A virtual admittance

ẍ∆ = M -1 (f M -B ẋ∆ -Kx ∆ ) (8)
is used at the local site to render a compliance with inertia M, stiffness K, and damping B. Thereby x ∆ is the offset between the desired end-effector position x d of one arm and the end-effector position x S received from the slave. Consequently, the input x d to the inner position control loop is determined as

x d = x ∆ + (x S -x B ), (9)
where x B is the position of the mobile platform relative to its starting position. The base is repositioned so as to always maximize the minimum manipulability of the two end-effectors. For the given kinematic structure ideal manipulability is given if both end-effectors are located on circles with fixed radius r opt around the shoulder joints. In addition, the first rotational joint of each interface should be controlled in such a way, that the minimum distance to their joint limits is maximized, i.e. the two joint angles must be equal.

The resulting configuration is symmetric and the corresponding platform position can be obtained by simple geometric calculations: the mobile platform must be aligned parallel to the connecting line from x L to x R and its center point must lie on the perpendicular bisector of the connecting line (see Fig. 4). The distance n opt can be computed using

n opt = r 2 opt - d -d P 2 2 + n P ( 10
)
where d = x Rx L is the vector connecting the two end-effectors in world coordinates, r opt is the radius of the circle with maximum manipulability, d P is the distance of the first rotational joints of the haptic interfaces, and n P is the distance of the platform center from the connecting line between the first rotational joints of the haptic interfaces. A PD controller is then used to drive the mobile base to the desired position. As the current base position is taken into account in the virtual admittance the positions received from remote site are displayed in world-coordinates at the local site.

Here only a short overview of the control scheme has been given. For details on the control of the haptic interfaces, see [START_REF] Peer | A New Admittance-Type Haptic Interface for Bimanual Manipulations[END_REF]. The mobile haptic interface is presented in [START_REF] Unterhinninghofen | Control of a Mobile Haptic Interface[END_REF].

V. EXPERIMENTS

A. Experimental Setup

The setup used for the experiments is depicted in Fig 5 ,  for a detailed description please refer to [START_REF] Peer | Intercontinental Multimodal Tele-Cooperation Using a Humanoid Robot[END_REF]. The humansystem interface, located in Munich, Germany, consists of the mobile haptic interface, described in Sec. IV, to allow haptic interaction and a head-mounted display to provide visual feedback to the operator. As teleoperator, located in Tsukuba, Japan, the humanoid robot HRP-2, as described in Sec. III, is used. The two narrow-angle cameras of HRP-2 record the remote scene. For control, forces and torques from both arms are sent from master to slave, while end-effector positions and orientation of both arms are sent from slave to master. At a transmission rate of 50 Hz, the packet loss was negligible (< 1%) and the round-trip time between Germany and Japan was approximately T = 280 ms.

B. Results and Discussion

Experiments in which both, the human operator and the teleoperator performed some steps have been successfully performed. Fig. 6 depicts the results for an experiment in which the human operator in Munich applies most of the force necessary to initiate the movement and the human collaborator in Tsukuba loosely holds the end-effectors of the teleoperator. A very good position tracking between the respective end-effectors is achieved (solid lines), also while walking. The slight deviations that can be seen are caused by time-delay in the communication channel and a desired compliant behavior of the haptic interfaces on master side. However, a large deviation of the body positions between the human operator and the teleoperator are observed (dashed lines). E.g. after 43 s the teleoperator starts moving backwards from the operator's point of view although the human operator is still moving forwards. Also, some low frequency oscillations of the teleoperator position that are caused by the current implementation of the walking pattern generator are evident. The human operator observes this as disturbing. Thus, a high degree of immersion and good task performance while walking were not achieved using this coupling scheme. Fig. 7 depicts the results for the reverse experiment, i.e. the human operator in Munich loosely holds the end-effectors and is guided by the human collaborator in Tsukuba. Position tracking of the end-effectors is equally good. Again, a deviation of the body positions is apparent. However, as the human collaborator on site notices oscillations caused by the walking pattern generator more quickly and reacts immediately, no low frequency oscillations of the teleoperator's body position can be observed.

Summarizing the experimental results it can be concluded that the goal of performing a teleoperation experiment, in which walking and haptic interaction occur simultaneously, has been achieved. Further improvements to this concept are however necessary to achieve a high degree of immersion and a good task performance. In both experiments, due to the current implementation of the walking pattern generator, the walking speed is relatively slow. The implementation of a faster walking pattern generator is in progress.

VI. CONCLUSION AND FUTURE WORK

In this paper, we presented the ongoing work in realizing a collaborative teleoperation task. We described how the local and remote platforms have been coupled, and what potential enhancements of this coupling scheme are. The local controllers on each site have been presented, with focus on the control of the humanoid robot. The experimental results show that the limitations of the walking gait generation of the teleoperated humanoid platform is the reason for large deviations between the postures of the operator and teleoperator. This might lead to a degradation of the immersion and task performance.

Hence, our future work will consist in improving the walking pattern generator and in the implementation of algorithms for predicting the motions of the human operator. To deal with the former problem, methods have already been proposed to cope with perturbations on the ZMP [START_REF] Nishiwaki | High Frequency Walking Pattern Generation Based on Preview Control of ZMP[END_REF] or to reduce the computation time [START_REF] Morisawa | Experimentation of Humanoid Walking Allowing Immediate Modification of Foot Place Based on Analytical Solution[END_REF]. However, in [START_REF] Morisawa | Experimentation of Humanoid Walking Allowing Immediate Modification of Foot Place Based on Analytical Solution[END_REF] a range of situations are given where the ZMP leaves the support polygon. We recently [START_REF] Dimitrov | An Optimized Linear Model Predictive Control Solver for Online Walking Motion Generation[END_REF] proposed a new solver for the QP program, which is the core of the preview control method under the constraints of the support polygon. This new solver allows to compute a new command in 2 ms on the HRP-2 robot, and it should be able to ensure the solution to lie inside the support polygon. Its inclusion into the overall system is in progress.

Another challenge will be to reflect the posture of the operator on the humanoid platform. As the teleoperator does not have the same kinematics as the operator, and the body control of the teleoperator is subject to very hard constraints, such as keeping balance in presence of disturbing forces, care will have to be taken to find a compromise between posture reflection and constraint violations.

Fig. 1 .

 1 Fig. 1. Schematic setup and components.

Fig. 2 .

 2 Fig. 2. Coordination scheme considering coupling of end-effectors only

Fig. 3 .

 3 Fig. 3. Coordination scheme considering coupling of bodies and endeffectors

Fig. 4 .

 4 Fig. 4. Geometric solution for optimal positioning of the mobile platform

Fig. 5 .Fig. 6 .

 56 Fig. 5. Photos of experimental setup. On the left the operator site with human operator and mobile haptic interface in Munich is depicted. On the right the teleoperator HRP2 and the human collaborator in Tsukuba are shown. The video of the corresponding experiment is attached to the paper.

Fig. 7 .

 7 Fig. 7. Position tracking of body and end-effectors with a dominant human collaborator at remote site. Data of the human operator at master site is depicted in black, data of the teleoperator in gray. The dashed lines represent the body position whereas the solid lines represent the right hand/endeffector.


S Puechmorel
email: stephane.puechmorel@enac.fr

Daniel Delahaye
email: delahaye@recherche.enac.fr

Functional interior point programming applied to the aircraft path planning problem

Multiple aircraft trajectory planning is a central problem in future air traffic management concepts where some part of the separation task, currently assumed by human controllers, will be delegated to on-board automated systems. Several approaches have been taken to address it and fall within two categories: metaheuristic algorithms or deterministic methods. The framework proposed here models the planning problem as a optimization program in a space of functions with constraints obtained by semi-infinite programming. A specially designed innovative interior point algorithm is used to solve it.

Introduction

The path planning problem for multiple robots evolving in a possibly dynamic environment with obstacles is a very active area of research among the automatics and robotics communities, and is addressed in many classical references [START_REF] Latombe | Robot Motion Planning: Edition en anglais[END_REF][START_REF] Lavalle | Planning Algorithms[END_REF]. For application to aircraft trajectories, nonholonomic constraints have to be considered since bounds on velocity and curvature are dictated by flight dynamics and aircraft operations. In such a context, even for two dimensional motion, it is known that an exact shortest path computation for a single mobile avoiding polyhedral obstacles is NP-Hard [START_REF] Canny | New lower bound techniques for robot motion planning problems[END_REF]. In order to make the problem tractable, only approximate solutions are to be sought after. One possible way of dealing with the intrinsic complexity is to resort to solvers based on metaheuristics. Since the state space exploration is made on a random fashion, there is no insurance on the quality of the solution obtained after a finite number of iterations and it is not even possible for some algorithms to prove convergence in a probabilistic sense. However, due to the nature of the metaheuristic approach, a wide class of constraints and optimality criteria can be taken into account. In the field of air traffic management, one can impose that the solution is built using only maneuvers used by human controllers. Successful applications of stochastic optimization algorithms can be found in [4,[START_REF] Durand | Ant colony optimization for air traffic conflict resolution[END_REF]. The second way of addressing the issue of computational complexity is to allow sub-optimality of the solution. Turning the original problem into a continuous optimization program under constraints allows traditional algorithms to be used and provides a locally optimal solution. As the NP-hardness of the original formulation cannot be avoided, global optimality cannot be reached generally, unless some kind of exhaustive search among all local optima is performed. Hybridization between the two approaches can be done in order to randomly sample the set of local optima, yielding in turn an improved admissible solution. The algorithm that will be presented here falls in the second category, and is built using a special geometrical property of complex configuration spaces. The path planning program is designed in a functional way, with criterion based on a cost associated to individual trajectories and constraints expressed using semi-infinite programming. The paper is organized as follows: in a first section, general results on complex configuration spaces will be briefly recapped, as they will provide the theoretical framework on which the interior point formulation will be built. The second section will introduce the optimization program associated to the path planning problem, and a relation with a recently introduced complex harmonic navigation function will be pointed out. Finally, a possible algorithmic implementation of the solver yielding an approximate solution to the original problem will be detailed.

Complex configuration spaces

Given a set of N mobiles with planar motion, the complex configuration space C N is defined to be the Cartesian product of n copies of the complex plane C with the set ∆ N = (z 1 , . . . , z N ) , ∃i = j, z i = z j of simultaneous positions removed. Any path connecting two points in C N is an admissible collision-free planning of N trajectories. It is easy to show that C N is a path connected space using a sequential planning argument: let z 0 1 , . . . , z 0 N be any initial configuration and let z 1 1 , . . . , z 1 N be the desired final situation. Let z 1 , . . . z N be fixed. Then it exists a path joining z 0 1 and z 1 1 in C-{z 1 , . . . z N } since this space is path-connected. The same procedure can then be applied iteratively to the remaining points. The resulting path is a sequence of moves along coordinates axis in C n and assumes that only one mobile moves at a time.

Geometrical insights about C N can be gained from the knowledge of its cohomology group, obtained in [START_REF] Arnol'd | The cohomology ring of the colored braid group[END_REF]. It turns out that the generators of this group will be used to define the constraints in the optimization program constructed later. They consist of degree one forms: thus admits an interpretation as an expansion rate (resp. rotation rate) for the vector d kl (t) = Γ k (t) -Γ l (t) and in turn the integral:

Γ dω k,l
will give log ( d kl (1) )log ( d kl (0) ) as its real part and 2πθ for its imaginary part, with θ the winding number of the path d kl . Considering the plane z k = z l that is a subset of ∆ N , one can interpret d kl (t) , for t ∈ [0, 1], as twice the distance of the path t → Γ k (t) (resp. t → Γ l (t)) to the constraint z k = z l . In the spirit of interior point algorithms, it is natural to considerlog ( d kl (t) ) as a barrier function for the constraint z k = z l , with the major difference that it gives rise to a mapping instead of a single value.

Basic assumptions for the path planning problem

As the path planning problem is targeted towards air traffic applications, some restrictions on the manoeuvers that an aircraft can do are coming from flight dynamics and passengers comfort. First of all, velocity has to be bounded below and from above, with a quite narrow interval of efficiency dictated by engines performance. Second, curvature cannot be made arbitrary high and it is advisable to limit its total integrated value so as to minimize its detrimental effect on passengers comfort.

It is not intended in this work to consider aircraft in terminal manoeuvering areas, nor the climb and descent phases: as a consequence, the path planning algorithm will not perform any change in altitude, yielding a problem that conforms to the complex configuration space modelling. Furthermore, only collision avoidance is considered, the compliance with separation norms will be addressed in a future work.

Finally, it is assumed that the planner is used in a free flight context where the aircraft are not bound to predefined routes and at a tactical level with a time horizon not exceeding 20 minutes. In such a case, one can assume that the level of uncertainty is low enough to allow a deterministic approach to be taken. The effect of wind will not be included in the model. This may be unrealistic at first glance since it is one of the most influential factor on aircraft trajectories, but given the ability of future FMS systems to infer and broadcast the wind experienced along the flight path, it seems reasonable that the wind field will be known with a sufficient degree of accuracy to adjust the initial planning.

Perfect knowledge of the aircraft positions within the airspace of interest is assumed, no communication issues are considered.

Path planning as a penalized optimization program

To turn the path planning problem into an optimization program amenable to interior point algorithms, it is needed to define first a usable criterion and second a mean of getting a tractable set of barrier functions that will be used to penalize the criterion. A major concern for the last point is the functional nature of the state space: a mean of turning the continuous time constraints into a vector of real value must be sought after.

Applicable criterion on a set of trajectories

For en-route traffic, overall flight cost is the dominant factor that airlines want to optimize. Since most of the time aircraft are flying near their efficient altitude and at constant velocity, it can be related to trajectory length or flight time. A second consideration that can be taken into account is the search for a flight path minimizing total curvature, as every turns induces an increase in fuel consumption and has a detrimental effect on passengers comfort.

Based on the previous remarks, a natural choice for the the optimization criterion is the cumulative length. Let a smooth path Γ : [0, 1] → C N in the configuration space for N planes be given. Its cumulative length is defined as:

L (Γ ) = N ∑ j=1 1 0 Γ ′ j (t) dt
In order to take into account possible priorities between aircraft, a weight may added:

L (Γ ) = N ∑ j=1 w j 1 0 Γ ′ j (t) dt
with w j ∈ [0, 1] and ∑ N j=1 w j = 1. Let ε be a positive value and H : ] -ε, ε[×[0, 1] → C N a smooth mapping such that for all t ∈ [0, 1], H(0,t) = Γ (t) and for all s ∈ ] -ε, ε[, H(s, 0) = Γ (0), H(s, 1) = Γ (1). The derivative:

∂ H ∂ s (0, •) : [0, 1] → C n
may be interpreted as a tangent vector at base point Γ in the manifold of smooth paths of C N [START_REF] Michor | Riemannian geometries on spaces of plane curves[END_REF]. The derivative of L applied to it can be computed from the definition:

∂ L(H(s, •)) ∂ s | s=0 = N ∑ j=1 w j 1 0 ∂ 2 H j ∂ s∂t (0,t), ∂ H j ∂t (0,t) ∂ H j ∂t (0,t)
dt using an integration by parts and, since the endpoints of H are fixed:

∂ H ∂ s (0, 0) = ∂ H ∂ s (0, 1) = 0 it comes: ∂ L(H(s, •)) ∂ s | s=0 = - N ∑ j=1 w j 1 0 ∂ H j ∂ s (0,t), κ i (t)N i (t) dt
where κ i is the curvature of the path Γ i and N i its unit normal vector. The expression obtained is the classical first order variation for the length of a smooth path [START_REF] Michor | Riemannian geometries on spaces of plane curves[END_REF] , summed over all components of the path Γ . In the language of manifolds,

∂ L(H(s, •)) ∂ s | s=0
gives the value obtained by applying the form dL to the tangent vector

Γ , ∂ H ∂ s (0, •)

Turning collision avoidance constraints into semi-infinite barrier functions

The results on complex configuration spaces presented in the first section make natural the choice of an integral form of the barrier function [START_REF] Shapiro | Semi-infinite progamming, duality, discretization and optimality conditions[END_REF]. For the constraint z i = z j it will be defined as:

- 1 0 log Γ i (t) -Γ j (t) dt
Please note that such an integral does not necessary take an infinite value if Γ i (t 0 ) = Γ j (t 0 ) and thus differs from the finite dimensional setting. However, it turns out that there is a close relationship between curvatures of the component paths Γ i and the norms Γ i (t) -Γ j (t) that can be used to make the barrier functions effective. Let H : ]-ε, ε[×[0, 1] → C N be a smooth mapping satisfying the same properties than in 3.1. The penalized criterion is:

N ∑ j=1 w j 1 0 Γ ′ j (t) dt -∑ j =k λ jk 1 0 log Γ j (t) -Γ k (t) dt
where the λ jk are strictly positive penalty weights. Let:

Θ (Γ ) jk = 1 0 log Γ j (t) -Γ k (t) dt
be the j, k barrier function. The first order variation formula for Θ (Γ ) jk gives:

∂Θ jk (H(s, •)) ∂ s | s=0 = 1 0 ∂ H j ∂ s (0,t) -∂ H k ∂ s (0,t),Γ j (t) -Γ k (t) Γ j (t) -Γ k (t) 2 dt
Reordering terms, the variation formula for the complete criterion is:

- N ∑ j=1 w j 1 0 ∂ H j ∂ s (0,t), κ i (t)N i (t) dt- N ∑ j=1 ∑ k = j λ jk 1 0 ∂ H j ∂ s (0,t) -∂ H k ∂ s (0,t),Γ j (t) -Γ k (t) Γ j (t) -Γ k (t) 2 dt
Assuming that the path Γ is minimal with respect to penalized criterion, the first order necessary optimality conditions yields, for all j = 1 . . . N:

w j κ j (t)N j (t) = ∑ k = j λ jk Γ k (t) -Γ j (t)) Γ j (t) -Γ k (t)
2 Some important facts may be derived from the expression above. First of all, since N i (t) is of unit norm, it comes:

w j |κ j (t)| ≤ ∑ k = j λ jk Γ j (t) -Γ k (t)
So that there is a control on the curvature given by the penalty weights. In the special case of a path planning problem with only two mobiles, that turns out to be associated in the frame of air traffic applications with two planes encounters, the relation becomes an equality:

w j |κ j (t)| = λ jk Γ j (t) -Γ k (t) , j = 1 . . . 2, k = j
In such a case, bounding the curvature of the paths will in turn bound the minimal distance separating the two mobiles: it improves over the simple collision avoidance guarantee by allowing separation norms to be enforced. The general situation with an arbitrary number of mobiles is not so simple to deal with. However, from the expression linking κ j (t)N j (t) and the sum of terms coming from the barrier functions, it is clear that a collision is not possible with two mobiles if the curvature is bounded (otherwise, the corresponding term will go to infinity while the remainder will be bounded). Only cases involving encounters with 3 or more mobiles and a special symmetry may violate this fact.

Addressing this issue can be done by noticing that the barrier functions were derived from the complex mapping log(z iz j ). It is in fact the complex potential generated by a simple sheet uniform distribution on the plane z j = z j . When encounters involving p > 2 mobiles are considered, the forbidden area in C N is an intersection of p -1 hyperplanes and has an expression:

∆ i 1 ,...,i k = {z : z i 1 = z i 2 = • • • = z i k }
where the sequence i 1 , = i 2 . . . , = i p is extracted from the complete set of indices 1 . . . N and denotes the mobiles in interaction. The complex potential generated by a simple sheet distribution on ∆ i 1 ,...,i k can be obtained using the procedure described below. First of all, the projection of a point z in the configuration space onto ∆ i 1 ,...,i k is given by: P i 1 ,...,i k (z) = z 1 , .., z i 1 -1 , h, z i 1 +1 , .., z i k -1 , h, z i k +1 , .., z n where h = k -1 ∑ k j=1 z i j is the mean value of the components belonging to the forbidden set. The complex potential generated by a simple sheet distribution is then: (2-k) This potential remains harmonic, but not pluriharmonic as the one based on the log.

k ∑ j=1 z i j -h 2-k = z -Pz 2
Taking the integral of z -Pz 2(2-k) along a path Γ in C N yields the additional barrier function needed to deal with the encounter situation described by ∆ i 1 ,...,i k . It is clear from the expression of this set that 2 N-1 terms will have to be taken into account in the final penalty term. It can be viewed as a consequence of the NP-Hardness of the original problem and shows how the intrinsic combinatorics of the optimization program will appear indirectly. For practical applications, is it extremely uncommon to consider encounters involving more than 4 mobiles: on a the French airspace, it happens only a few times a year. Exceeding 5 occurs only on simulated traffic. The choice made was thus not to include the extra barrier terms and to postpone the complete formulation to a future work, where a conflict detection algorithm will be launched in a pre-processing phase in order to keep only the really needed high order barrier functions.

Interior point solver

The solver is implemented using a very simple procedure that consists in discretizing the trajectories at points regularly located in the interval [0, 1]. The flight path of aircraft i, i = 1 . . . N is then described as a sequence x i, j = Γ i ( j/m) where m is the number of samples on each trajectory. Following the derivations made in [START_REF]Path planing for multiple robots: an alternative duality approach[END_REF], a maximum curvature κ max can be imposed using the relation:

x i, j+1 -x i, j-1 x i, j -x i, j-1 ≥ 4 -κ 2 max x i, j -x i, j-1
Under the near constant velocity assumption:

x i, j -x i, j-1 ≈ L i m
with L i the total length of Γ i , and the expression reduces to a lower bound condition on x i, j+1x i, j-1 . The integrals involved in the expression of the functional criterion turn to finite sums, in a way amenable to standard non-linear optimization algorithms. The expression of the gradient of the penalized criterion at a single vertex x i, j may be obtained using the following approximation of the curvature and normal vector, again assuming constant velocity and putting l i = L i /m:

• |κ i ( j/m)| = x i, j+1 -2x i, j + x i, j-1 /l i • N i ( j/m) = x i, j+1 -2x i, j +x i, j-1 x i, j+1 -2x i, j +x i, j-1
The gradient is then, with respect to the i-th trajectory:

w i κ i ( j/m)N i ( j/m) + ∑ k =i λ i,k x i, j -x k, j x i, j -x k, j 2
and summing up all the contribution gives rises to the complete gradient of the penalized criterion, that is used within a classical finite dimensional optimizer.

Conclusion and future work

A framework based on a functional description of the path planning problem has been described. Specially designed barrier functions inspired both by complex potential theory and semi-infinite programming allows to turn the original problem with an infinite number of constraints into a more tractable one, involving only a finite number of integrals. Bounding the curvature below allows to ensure collision avoidance. A possible implementation of the algorithms based on sampling trajectories in an even fashion was described. The complete procedure is in an early stage of development and a large amount of work still has to be performed:

• Pre-process the input traffic so as to identify encounters geometries in order to be able to use higher order barrier functions without incurring too much computational complexity. • Investigate alternative representations of trajectories, especially those based on expansions on functional basis. • Perform an extensive benchmark test on both synthetic and real traffic so as to assess the performance of the algorithm. A special attention must be paid to sensitivity analysis and robustness assessment against random perturbations. • Integrate the algorithm within a realistic trafic simulator so as to quantify the effects of flight path tracking errors.

ω k,l = dz kdz l z kz l , 1 ≤ k ≤ N, 1 ≤ l ≤ N, k = lPlease note that in the original paper[START_REF] Arnol'd | The cohomology ring of the colored braid group[END_REF] a factor (i2π) -1 appears in the expression: it is removed here as the real part of ω k,l is of primary interest. Let a C 1 path Γ : [0, 1] → C N be given. Then:Γ dω k,l = [0,1] Γ ′ k (t) -Γ ′ l (t) Γ k (t) -Γ l (t) dtwith Γ j , j = 1 . . . N denoting the j-th component of Γ . Rewriting the right hand side, it comes:Γ dω k,l = [0,1] Γ ′ k (t) -Γ ′ l (t) Γ k (t) -Γ l (t) |Γ k (t) -Γ l (t)| 2 dtGiven any two complex numbers z 1 = x 1 + iy 1 , z 2 = x 2 + iy 2 , the real part of the product z 1 z 2 is the inner product of the vectors (x 1 , y 1 ) and (x 2 , y 2 ) and imaginary part their determinant. The expression:Γ ′ k (t) -Γ ′ l (t) Γ k (t) -Γ l (t) |Γ k (t) -Γ l (t)| 2