Alexander Sherikov

Dimitar Dimitrov

Pierre-Brice Wieber

Whole body motion controller with long-term balance constraints

The standard approach to real-time control of humanoid robots relies on approximate models to produce a motion plan, which is then used to control the whole body. Separation of the planning stage from the controller makes it difficult to account for the whole body motion objectives and constraints in the plan. For this reason, we propose to omit the planning stage and introduce long-term balance constraints in the whole body controller to compensate for this omission. The new controller allows for generation of whole body walking motions, which are automatically decided based on both the whole body motion objectives and balance preservation constraints. The validity of the proposed approach is demonstrated in simulation in a case where the walking motion is driven by a desired wrist position. This approach is general enough to allow handling seamlessly various whole body motion objectives, such as desired head motions, obstacle avoidance for all parts of the robot, etc.

I. INTRODUCTION

Humanoid robots have to perform tasks that are typically expressed as high-level objectives involving whole body motion control. For example, approaching and manipulating objects, maintaining visual contact, [START_REF] Mansard | Visuallyguided grasping while walking on a humanoid robot[END_REF] etc. The motion of a robot must also respect feasibility constraints, such as collision avoidance, satisfaction of various mechanical constraints, and keeping balance. Fulfillment of the highlevel objectives simultaneously with feasibility constraints often requires planning motion in advance, in particular when locomotion is performed. Due to the high complexity of humanoid robots, real-time control schemes typically resort to planning based on approximate models. Though these models often reduce a robot to a single point-mass, they were demonstrated to be very effective in preserving longterm balance [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF], and even in choosing appropriate feasible footstep positions [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF], [START_REF] Diedam | Online walking gait generation with adaptive foot positioning through linear model predictive control[END_REF].

The typical sequence of real-time motion generation [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF], [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF], [START_REF] Morisawa | Experimentation of humanoid walking allowing immediate modification of foot place based on analytical solution[END_REF], [START_REF] Nishiwaki | Online walking control system for humanoids with short cycle pattern generation[END_REF] includes: (i) first plan the motion of the center of mass with an approximate model to ensure long-term balance, (ii) then control the whole body of the robot to execute this plan taking into account whole body motion objectives and constraints. In this scheme the plan that results from stage (i) may not satisfy all the constraints considered at stage (ii), since they are usually neglected in the approximate model. Though an infeasible plan may still be executed approximately [START_REF] Kanehiro | Feasible pattern generation method for humanoid robots[END_REF], it is always preferable to avoid such situations. Furthermore, inaccuracies in execution of the plan require its periodic corrections [START_REF] Buschmann | A method for real-time kineto-dynamic trajectory generation[END_REF], [START_REF] Takenaka | Real time motion generation and control for biped robot -3rd report: Dynamics error compensation[END_REF]. Also, the high-level objectives cannot in general be expressed with the limited approximate model at stage (i), even though they might be required to drive the locomotion of the robot, for example, approaching an object before grasping it [START_REF] Yoshida | Task-driven support polygon reshaping for humanoids[END_REF]. This lead to developing ad hoc approaches [START_REF] Yoshida | Task-driven support polygon reshaping for humanoids[END_REF], [START_REF] Nishiwaki | Online humanoid walking control system and a moving goal tracking experiment[END_REF], [START_REF] Fukumoto | Hand-centered whole-body motion control for a humanoid robot[END_REF], which are often tailored to specific objectives and may be difficult to combine with footstep adjustments in response to external disturbances. Here, we propose to address the identified problems by omitting stage (i). Since stage (i) is crucial for long-term balance preservation, the whole body motion controller of stage (ii) must be modified to compensate for this omission.

The balance of humanoid robots can be analyzed in terms of viability theory and capturable states [START_REF] Aubin | Viability theory[END_REF], [START_REF] Wieber | Viability and Predictive Control for Safe Locomotion[END_REF], [START_REF] Koolen | Capturability-based analysis and control of legged locomotion, part 1: Theory and application to three simple gait models[END_REF]. A robot is said to be in a capturable state, if it can eventually stop all motion and avoid falling or violating other feasibility constraints. In order to preserve balance the robot should never leave the set of capturable states, which is unfortunately impossible to determine in general [START_REF] Wieber | Viability and Predictive Control for Safe Locomotion[END_REF]. However, it is possible to check with an approximate model if the robot can stop in K steps, i.e., if a state of this model is K-step capturable [START_REF] Koolen | Capturability-based analysis and control of legged locomotion, part 1: Theory and application to three simple gait models[END_REF]. Therefore, in order to preserve capturability, the motion can be previewed for K steps, with a terminal stopping constraint [START_REF] Morisawa | Experimentation of humanoid walking allowing immediate modification of foot place based on analytical solution[END_REF], [START_REF] Urata | Online walking pattern generation for push recovery and minimum delay to commanded change of direction and speed[END_REF], [START_REF] Takenaka | Real time motion generation and control for biped robot -1st report: Walking gait pattern generation[END_REF]. Clearly, at least one step must be considered to enable walking (K ≥ 1).

Here we propose a modification of the standard whole body controller in such a way, that it does not require preliminary motion planning. In order to achieve this without deteriorating the long-term balance, the controller is augmented with appropriate constraints, which rely on motion preview with an approximate model to verify Kstep capturability. Thus the controller generates such whole body commands that the balance of the previewed motion is preserved. At the same time the previewed motion is compliant with whole body constraints and objectives. The approximate model adopted in this work enables automatic adaptation of footstep positions as in [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF], [START_REF] Urata | Online walking pattern generation for push recovery and minimum delay to commanded change of direction and speed[END_REF], but here, the adaptation is directly driven by the whole body motion objectives instead of a reference velocity for the body. This behavior is similar to the one demonstrated in [START_REF] Kanoun | Planning foot placements for a humanoid robot: A problem of inverse kinematics[END_REF], but does not require offline computations. Simultaneously with realization of whole body motion objectives, our controller automatically adapts footsteps positions when necessary to compensate for external disturbances, which is not addressed in [START_REF] Yoshida | Task-driven support polygon reshaping for humanoids[END_REF], [START_REF] Nishiwaki | Online humanoid walking control system and a moving goal tracking experiment[END_REF], [START_REF] Fukumoto | Hand-centered whole-body motion control for a humanoid robot[END_REF]. Some works advocate for motion preview with nonlinear whole body models [START_REF] Erez | An integrated system for real-time model predictive control of humanoid robots[END_REF]. While such an approach is potentially more descriptive, due to computation time limits it is still necessary to employ approximations whose validity on real robots has yet to be demonstrated. And, more importantly, long-term balance constraints are not enforced as we explicitly do here.

The proposed whole body motion controller is posed as a hierarchical linear least squares problem with inequality constraints, which is solved using a dedicated active set solver [START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF], [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF].

The building blocks of the optimization problem are described in the Sections II-V: Section II briefly reviews standard whole body motion constraints, Section III outlines long-term balance constraints based on an approximate model, Section IV describes the coupling between the approximate and whole-body models, motion objectives are described in Section V. Finally, Section VI introduces the hierarchical least squares formulation of the whole body motion control, while Section VII presents simulation results.

II. STANDARD WHOLE-BODY MOTION CONSTRAINTS

Our whole body controller is based on the standard inverse dynamics approach [START_REF] Fujimoto | Proposal of biped walking control based on robust hybrid position/force control[END_REF], which allows the definition of the instantaneous whole-body motion feasibility constraints as described in this section.

A. Lagrangian dynamics

The dynamics of a walking robot can be described as

H 1 H 2 H q + h 1 h 2 h = τ 0 + J T c,1 J T c,2 J T c f , (1)
where H is the generalized inertia matrix; q = (q ′ , q ′′ ) is the vector of generalized coordinates including joint angles q ′ and position and orientation of the base of the robot q ′′ ; h is the vector of Coriolis, centrifugal, and gravity effects; τ is the vector of joint torques; f is the vector of ground contact forces; and J c is the corresponding Jacobian matrix. Note that given any q and f , joint torques τ satisfying the first part of dynamics can always be found. Consequently, we only need to find q and f such that

H 2 q + h 2 = J T c,2 f . (2)

B. Constraints on the contact forces

The constraints on the ground contact forces reflect two aspects: limitations due to Coulomb's friction, and restriction of the Center of Pressure (CoP) to the contact area [START_REF] Fujimoto | Proposal of biped walking control based on robust hybrid position/force control[END_REF]. For simplicity, we describe the constraints only in single support, while extension to double support is trivial.

The contact forces f are considered as a single wrench, applied at a reference point on the contact surface. The constraints on this wrench are formulated in a local frame located at the reference point, with the z axis normal to the contact surface. Thus, the wrench is expressed as

f = T f c µ c , (3)
where f c and µ c are the force and moment applied at the reference point in the local frame, and T is a frame transformation matrix. We use a linear approximation of the Coulomb's friction cone, considering feasible contact force as a conic combination of unit vectors lying on the edges of the cone

f c = Gλ, ( 4
)
where λ is a vector of positive coefficients

λ ≥ 0. (5)
At the same time, the moment about the z axis is limited with

µ z c ≤ γf z c , (6)
where γ is a torsional friction coefficient ( [START_REF] Murray | A Mathematical Introduction to Robotic Manipulation[END_REF], Section 5.5).

Position of the CoP is kept within the support area with a constraint of the form [22]

A f c µ c ≤ b. (7)

C. Maintaining contacts

In order to ensure that the robot does not break contacts with the ground, we need the acceleration of each support foot to be zero in both translation and rotation:

J c q + Jc q = 0. (8)

D. Joint limits

The joints angles q ′ must stay within their mechanical limits. This is taken into account with the constraints

q ′ ≤ q ′ + T q′ + T 2 2 q′ ≤ q ′ , (9)
where q ′ and q ′ are the vectors of lower and upper joint limits, and T is a positive time parameter.

III. LONG-TERM BALANCE CONSTRAINTS

The whole body constraints reviewed in Section II guarantee only instantaneous feasibility of the motion, and, in particular, they are not sufficient for long-term balance preservation. For this reason, it is common to impose additional constraints, for example, restricting the projection of the Center of Mass (CoM) to the support polygon when standing still [START_REF] De Lasa | Feature-based locomotion controllers[END_REF]. The balance constraints described in this section are based on a preview of the motion of an approximate model and can be used both for walking and standing.

There is a variety of approximate models developed for planning the motion of humanoid robots. Many use a linear single point-mass model [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF], some introduce additional point-masses [START_REF] Takenaka | Real time motion generation and control for biped robot -1st report: Walking gait pattern generation[END_REF], or constant external forces [START_REF] Ibanez | Unified preview control for humanoid postural stability and upper-limb interaction adaptation[END_REF], or angular momentum [START_REF] Lee | Reaction mass pendulum (rmp): An explicit model for centroidal angular momentum of humanoid robots[END_REF]. Even the simple point-mass model can be used differently: as a second or third order model [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF], [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF], [START_REF] Englsberger | Bipedal walking control based on capture point dynamics[END_REF]. The latter one appears to be more suitable, since it allows for smooth transition of the contact forces [START_REF] Kajita | Biped walking stabilization based on linear inverted pendulum tracking[END_REF]. Our approach is not limited to a certain model, and here, for the sake of illustration, we adopt a simple third order point-mass model. This model enables automatic footstep placement, provided that the duration and sequence of steps are predetermined to keep the model linear [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF].

A. Point-Mass Model

The motion of the point-mass model along the x axis is characterized by the continuous time system

  ċx cx ... c x   =   0 1 0 0 0 1 0 g c z 0     c x ċx cx   +   0 0 -g c z   żx , z x = c x - c z g cx , (10)
where c = (c x , c y , c z ) is the position of the CoM of the robot, z = (z x , z y ) is the position of the Center of Pressure (CoP) on the ground and g is the gravitational acceleration, aligned with the z axis. This system is equivalent for the motion along y axis and is derived under assumptions, that the robot walks on a flat ground and the CoM height c z is constant. System ( 10) is discretized and its evolution is previewed over N time intervals. Since evolution of the system is determined by its initial state (c x,y 0 , ċx,y 0 , cx,y 0 ) and the control inputs ( ż0 , . . . , żN-1 ), all constraints and objectives can be expressed using them. The control inputs in turn can be expressed through the CoP and footstep positions in order to simplify constraints as described in the following.

B. Constraints on positions of the CoP

A walking motion on a flat ground is feasible only if the CoP remains inside the support polygon [START_REF] Wieber | On the stability of walking systems[END_REF]. This requirement can be taken into account explicitly in the form of a constraint on the position Z ∈ R 2N of the CoP over the next N preview sampling intervals. Provided that the feet are rectangular and the CoP positions Z are expressed in appropriately chosen frames, the constraints on Z can be expressed

Z ≤ Z ≤ Z, (11)
where Z and Z are the vectors of lower and upper bounds [START_REF] Dimitrov | Walking motion generation with online foot position adaptation based on ℓ 1 -and ℓ∞-norm penalty formulations[END_REF].

C. Constraints on positions of the feet

The key feature of the Model Predictive Control (MPC) scheme proposed in [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF] is that it automatically determines appropriate footsteps. In order to realize this behavior, the previewed footstep positions must be constrained to feasible areas with respect to the current foothold. These feasible areas are usually determined experimentally as polygons on the ground [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF]. Approximating them by rectangles and choosing reference frames appropriately allows formulating these constraints as simple bounds [START_REF] Diedam | Online walking gait generation with adaptive foot positioning through linear model predictive control[END_REF] P ≤ P ≤ P ,

where P ∈ R 2K is the vector of positions of the next K footsteps, P and P the corresponding vectors of lower and upper bounds.

D. Terminal capturability constraint

In order to enforce K-step capturability [START_REF] Koolen | Capturability-based analysis and control of legged locomotion, part 1: Theory and application to three simple gait models[END_REF], we impose a terminal stopping constraint at the end of the previewed K steps. It is well known, that for the second order point-mass model, the terminal capturability constraint involves position of the capture point

ξ x,y = c x,y N + 1 ω ċx,y N , (13)
which is constrained to stay in the final support area [START_REF] Koolen | Capturability-based analysis and control of legged locomotion, part 1: Theory and application to three simple gait models[END_REF], [START_REF] Takenaka | Real time motion generation and control for biped robot -1st report: Walking gait pattern generation[END_REF], [START_REF] Englsberger | Bipedal walking control based on capture point dynamics[END_REF]. An equivalent terminal constraint for the third order system (10) must be imposed on the velocity of the capture point

ξx,y = ċx,y N + 1 ω cx,y N = 0, (14)
from which follows ξ x,y = z x,y N . This implies that the capture point is again limited to the final support area, due to the constraints on the CoP positions [START_REF] Nishiwaki | Online humanoid walking control system and a moving goal tracking experiment[END_REF]. The equality ( 14) can be easily obtained by deriving the condition under which the system ( 10) is stable with a zero control input ż = 0.

Note that in some situations the capturability constraint can be infeasible, since it is impossible to stop in K previewed steps. This, however, does not necessarily imply that the robot will eventually fall. This issue is addressed in the controller proposed in Section VI.

IV. COUPLING THE APPROXIMATE AND WHOLE BODY

MODELS

The point-mass and the whole body models presented in Sections II and III are coupled through the current accelerations of the CoM and swing foot. The coupling ensures that the commands for the robot comply with the capturability constraint [START_REF] Wieber | Viability and Predictive Control for Safe Locomotion[END_REF], while the previewed motion fulfills the whole body constraints (Section II) and objectives (Section V).

A. Acceleration of the CoM

The CoM acceleration c0 can be directly obtained from the initial state of the system in [START_REF] Yoshida | Task-driven support polygon reshaping for humanoids[END_REF]. The CoM of the robot and the point-mass model are therefore linked by

J com q + Jcom q = c0 . ( 15
)

B. Acceleration of the swing foot

Given the next footsteps P and the current state of the swing foot (s 0 , ṡ0 ) (based on the current state of the robot), the swing foot trajectory is obtained using cubic polynomials as in [START_REF] Nishiwaki | Online walking control system for humanoids with short cycle pattern generation[END_REF]. This determines the current acceleration of the swing foot s0 as a linear function of (P , s 0 , ṡ0 ). Thus, during a single support, the swing foot follows this translational acceleration while maintaining a constant orientation:

J foot q + Jfoot q = s0 0 . (16)
V. CONTROL OBJECTIVES Various control objectives can be introduced in the controller, depending on the desired behavior. This section describes a few possible objectives, which were used in the simulations presented in Section VII.

A. Hand motion

A saturated PD controller π hand is used to drive the right hand to a desired position. This is realized using a least squares objective J hand q + Jhand qπ hand 2 .

(17)

B. Orientation of the torso

In order to prevent inclination of the body while walking, the orientation of the torso is kept constant with the objective J torso q + Jtorso qπ torso 2 .

(18)

C. Controlling joints

A least squares objective based on a simple joint PDcontroller

q′ + K d q′ -K p (q ′ ref -q ′ ) 2 ( 19
)
is used for two purposes: damping (K d > 0, K p = 0) and maintaining reference configuration

q ′ ref (K d > 0, K p > 0).

D. Penalizing violent swing foot motion

The footstep adjustments are allowed during the whole duration of a step, which may result in violent position changes near the end of a step. A straightforward workaround is to block footstep adaptation when the swing foot approaches the ground. Alternatively, it is possible to minimize the swing foot jerk ...

s 2 , ( 20
)
which is a linear function of (P , s 0 , ṡ0 ).

E. CoP positioning and damping

Also, we employ objectives in order to shape the previewed motion of the approximate model. It is done by minimizing the distance between the CoP positions and the center of the footsteps

Z -V P 2 , ( 21
)
where V is a matrix which selects footstep positions appropriately. Additionally, the CoP velocity

Ż 2 (22)
is minimized.

VI. A HIERARCHICAL LEAST-SQUARES PROBLEM

The constraints and objectives described in Sections II, III, IV, and V are collected in a single hierarchical optimization problem (or, more concisely, a hierarchy), which provides a convenient framework for whole body motion control [START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF]. All equality and inequality constraints imposed in a hierarchy are satisfied in a least-squares sense, i.e., the norm of their violation is minimized. For this reason, we can express the objectives described in Section V as equality constraints and introduce them at lower levels of a hierarchy.

The proposed whole body motion controller uses a hierarchy with the following three levels:

1) The highest priority level of the hierarchy includes the whole body motion constraints from Section II, long-term balance constraints described in Section III without the capturability constraint, and coupling constraints introduced in Section IV:

(

2) The capturability constraint [START_REF] Wieber | Viability and Predictive Control for Safe Locomotion[END_REF] is considered separately to prevent violation of the primary constraints when it is infeasible. Indeed, infeasibility of the capturability constraint does not always imply that the robot will fall, since it enforces capturability with a fixed preview horizon, which may be overly conservative.

3) The third level collects objectives, which are weighted to obtain the desired behavior: [START_REF] Takenaka | Real time motion generation and control for biped robot -1st report: Walking gait pattern generation[END_REF][START_REF] Kanoun | Planning foot placements for a humanoid robot: A problem of inverse kinematics[END_REF][START_REF] Erez | An integrated system for real-time model predictive control of humanoid robots[END_REF][START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF][START_REF] Fujimoto | Proposal of biped walking control based on robust hybrid position/force control[END_REF][START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF].

The decision variables of this hierarchy are the vector of accelerations of the generalized coordinates q = ( q′ , q′′ ), the contact wrenches expressed with λ and µ c , the positions of the CoP Z and footsteps P in the preview horizon, and the CoM acceleration cx,y 0 .

VII. SIMULATIONS

We evaluate the proposed approach in several simulations with an HRP-2 robot assuming perfect model and perfect inertial measurement unit. The commands for the robot are produced by solving the hierarchy introduced in Section VI with a dedicated active set solver on each control iteration. Size of the hierarchy depends on the length of preview horizon and discretization interval of the approximate model, which are set to the duration of two steps 0.8 • 2 (s) and 100 (ms) respectively as in [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF]. Such choice results in a hierarchy with about 120 equality and 80 inequality constraints and the number of decision variables changing between 80 and 90 depending on the state of the robot. In order to achieve high computational performance, the solver is hot-started using a guess of the active set and solution. Furthermore, the solver handles simple bounds on variables more efficiently than general inequality constraints, which is taken into account in the formulation of the hierarchy.

During the tests the robot is exposed to external disturbances whose implementation is described in the next subsection, while the following subsections focus on describing the test setups and results.

A. Simulating disturbances

External disturbances are modeled as instantaneous impacts leading to change in velocities. The relation between the initial qand resultant velocities q+ and impulses due to interaction with environment can be obtained by integrating the Lagrangian dynamics (1) [START_REF] Chareyron | Stability and regulation of nonsmooth dynamical systems[END_REF] where r d is an impulse representing the disturbance, r c is an impulse appearing due to ground contacts, J c and J d are respective Jacobians. The computed resultant velocities are constrained to maintain contacts

H( q+ -q-) = J T c r c + J T d r d , (23)
J c q+ = 0. (24)
All disturbances are applied to the torso of the robot and their directions are parallel to the ground.

B. Test 1: reaching an object

In this test we demonstrate walking driven by a wholebody objective, and more precisely the hand motion objective [START_REF] Takenaka | Real time motion generation and control for biped robot -1st report: Walking gait pattern generation[END_REF], which controls the right hand of the robot to a target position. This objective influences the walking motion generation, resulting in a sequence of steps taken towards the target. Note that exclusion of (17) from the hierarchy results in walking in place. The desired position of the hand is set to a point which is initially unreachable, while later during the simulation the desired position changes in an unpredictable way. The initial and final positions of the robot and target are shown in Fig. 1. The resulting motions of the CoM, feet and hand are shown in Fig. 3(a) and in the accompanying video. The reaction of the system to changes in the target hand position can be seen in Fig. 4. The robot starts walking since the target is initially unreachable, and continues to walk until the target is reached, around 4 (s). However, due to a change in the x position of the target, the walk is resumed (Fig. 4(a)). The lateral motion of the target also influences the walk, as can be seen in Fig. 4(b). This experiment clearly demonstrates that the walking motion (both CoM and feet motion) is determined taking into account wholebody objectives directly, which cannot be achieved with other schemes like the one in [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF]. It is demonstrated here with the desired hand position, but with respect to earlier similar results such as in [START_REF] Fukumoto | Hand-centered whole-body motion control for a humanoid robot[END_REF], the approach proposed here is not limited to the hand: it is completely generic and can handle seamlessly all sorts of whole-body objectives, such as a desired head motion, obstacle avoidance for all parts of the robot, etc. Moreover, we can see in Fig. 4(a) that with our approach, the walking motion reacts immediately, including footstep modifications even if the swing foot is in mid-air, as happens around 4 (s) and 8 (s).

In order to assess applicability of the proposed control approach to real-time applications, we measured computation time of the solver at each control iteration (see Fig. 2). Most of the time solver does only one iteration due to successful guess of the active set. The number of iterations increases during transition between the left and right supports. The average time required for solution of the optimization problem is equal to 0.92 (ms), when the solver is executed on Intel Core i5-3360M (2.80GHz) CPU.

C. Test 2: reaching an object and reacting to disturbances

The setup of this test is the same as for the previous one, but additionally the robot is pushed, while it is trying to approach the hand target. Disturbances of 15 Ns are applied to the robot at 2.5 (s) from the right, forcing the robot to make a step to the left, and at 7 (s) from the front, forcing the robot to make a step backward. The results can be seen in Figs. 3(b) and 5, and in the accompanying video. We can see, especially in comparison with the previous simulation without perturbations, that the controller immediately reacts to disturbances and successfully compensates for them by adjusting footsteps in mid-air.

The average time required for solution of the optimization problem is the same as before, but there are peaks up to 20 (ms) when disturbances are applied due to increased number of iterations of the solver. This issue can be addressed by limiting the number of iterations.

VIII. CONCLUSION

We presented a new approach to real-time whole body motion control, which, contrary to the standard approach [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF], [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF], [START_REF] Morisawa | Experimentation of humanoid walking allowing immediate modification of foot place based on analytical solution[END_REF], [START_REF] Nishiwaki | Online walking control system for humanoids with short cycle pattern generation[END_REF], does not perform preliminary motion planning with approximate models. The proposed controller design allows for generation of walking motions that satisfy longterm balance constraints and whole body motion objectives. This property is verified in simulations performed with HRP-2 robot. The computational performance demonstrated during the simulations is comparable to the performance of the state of the art real-time whole body motion controllers, which require preplanned footstep positions [START_REF] Kuindersma | An efficiently solvable quadratic program for stabilizing dynamic locomotion[END_REF].

  (a) Start of the simulation. (b) End of the simulation.

Fig. 1 .

 1 Fig. 1. Tests 1,2: Initial and final positions of the robot and the target.

Fig. 2 .

 2 Fig. 2. Test 1: Time required for solution of the optimization problem (above) and the number of iterations of the solver, i.e., the number of active set changes (below).

(a) Test 1 :

 1 No disturbances. (b) Test 2: With disturbances.

Fig. 3 .

 3 Fig. 3. Tests 1,2: Top view of the walking motion. Footsteps are represented by rectangles, trajectory of the CoM is in black. Trajectory of the hand is in dashed blue, while the trajectory of the target is in dotted red.

Fig. 4 .

 4 Fig. 4. Test 1: Reaction to changes in target position.

Fig. 5 .

 5 Fig. 5. Test 2: Reaction to changes in target position and external disturbances. The instants of disturbances are indicated with vertical red lines.


A hierarchical approach to minimum-time control of industrial robots

Saed Al Homsi * , Alexander Sherikov † , Dimitar Dimitrov † and Pierre-Brice Wieber † * Adept Technology France saed.al-homsi@adept.com † INRIA Rhône-Alpes, 38334 Montbonnot Cedex, France {alexander.sherikov, dimitar.dimitrov, pierre-brice.wieber}@inria.fr

Abstract-A novel approach to minimum-time control is presented. It is stated in terms of a hierarchical optimization problem, which is standard in the field of robotics. This is advantageous as already existing tools can be used to approach its solution. Our formulation is applied to the online generation of trajectories for industrial robots performing pick and place operations in the presence of obstacles. Model predictive control is used in order to achieve reactive system behavior and to obtain accurate local approximations of the collision avoidance constraints (which are nonconvex). Our approach has the capacity to suppress high frequency chattering in the control signal in the presence of noise: a common drawback of aggressive control strategies. Experiment using two SCARA robots that share the same working environment is used to evaluate the presented approach.

I. INTRODUCTION

This work addresses the problem of online trajectory generation for an industrial manipulator performing pick and place operations in the presence of dynamic obstacles. Since in many mechatronic applications the control input cost is less important than the task execution time [START_REF] Van Den Broeck | A model predictive control approach for time optimal point-to-point motion control[END_REF], we focus on fast transitions by attempting to achieve time-optimality. The user input for the proposed scheme is simply the desired endpoints without the need to specify an intermediate trajectory. This can simplify greatly deployment of industrial technology, leading to decreased cost and thus may have impact on various industrial applications [START_REF] Andreasson | Autonomous transport vehicles: where we are and what is missing[END_REF]. Accounting for the full-body dynamics when generating this intermediate trajectory is usually not essential as most industrial robots are position controlled. That is why we model the evolution of the joint positions and velocities of the manipulator using a discrete-time linear dynamical system while accounting for input and state constraints.

Since the collision avoidance constraints are in general nonconvex, we employ a Sequential Quadratic Programming (SQP) type of approach [START_REF] Nocedal | Numerical Optimization[END_REF] where a sequence of linearized sub-problems is solved. Each sub-problem1 identifies a minimum-time trajectory from the current state of the robot with respect to local linear approximations of the collision avoidance constraints. While such a sequence of problems is not guaranteed to converge to a time-optimal solution for the original nonconvex problem, it provides a practical way Fig. 1: Experimental setup. Two Adept Cobra SCARA [START_REF]Omron Adept Technologies, Inc[END_REF] robots sharing the same working environment. of generating locally optimal solutions, which is sufficient for most applications [START_REF] Kim | Near-minimum-time control of asymmetric rigid spacecraft using two controls[END_REF], [START_REF] Kim | A robust, discrete, near time-optimal controller for hard disk drives[END_REF]. Due to its local nature, our approach scales well with the number of manipulators and their degrees of freedom. In particular, we avoid the "curse of dimensionality" of global approaches (which usually rely on offline computations) [START_REF] Bertsekas | Dynamic Programming and Optimal Control[END_REF], [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF]. Our approach is applied in a Model Predictive Control (MPC) setting, which not only improves reactivity of the system but presents a possibility to obtain accurate local linear approximations of the collision avoidance constraints.

Our main contribution is the introduction of a hierarchical formulation [START_REF] Dimitrov | Multi-objective control of robots[END_REF] which guarantees time-optimal trajectories for the above mentioned SQP sub-problems. In more abstract terms, we address the problem of driving the state of an arbitrary discrete-time linear dynamical system to the origin in minimal time in the presence of linear constraints on inputs and states. Even though, in special cases, analytical solutions to this problem have been proposed [START_REF] Gao | On discrete time optimal control: a closed-form solution[END_REF], [START_REF] Zanasi | Nonlinear filters for the generation of smooth trajectories[END_REF], [START_REF] Zanasi | Discrete minimum time tracking problem for a chain of three integrators with bounded input[END_REF], the general case that we consider here necessitates the use of numerical techniques. In contrast to other numerical approaches [START_REF] Chen | The minimum-time problem for discrete-time linear systems: a non-smooth optimization approach[END_REF], we pose a problem that: (i) does not rely on the ad hoc selection of weighting factors (which is highly non-trivial), (ii) does not lead to any approximation and results in time-optimal behavior for arbitrary linear constraints (iii) and yet it is tractable in real-time. Our formulation hinges on recent developments of efficient hierarchical solvers in the field of robotics [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF], [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF] and can be integrated seamlessly in existing hierarchical control frameworks.

Apart from introducing our hierarchical formulation to minimum-time trajectory generation, we discuss practical issues related to its application. One such issue is the high frequency chattering in the control signal in the presence of noise when the setpoint has been reached [START_REF] Zanotto | Experimental validation of minimum time-jerk algorithms for industrial robots[END_REF]: a common drawback of aggressive control strategies. Following the ideas in [START_REF] Van Den Broeck | A model predictive control approach for time optimal point-to-point motion control[END_REF] we formulate our controller in a way that leads to smooth behavior in the vicinity of the goal state.

We present an experimental evaluation of the proposed approach using a typical industrial setup where two manipulators share the same working environment (see Fig. 1). Each manipulator has its own controller and considers the other manipulator as a potential obstacle. This is a problem of practical interest and presents a very good test bed for our approach due to the limited computational resources (the underlying optimization problem for each manipulator is solved on a CPU of 400 MHz under the constraint that not more than half of the CPU power can be utilized).

The paper is organized as follows. Section II reviews a classical approach to the minimum-time control problem and introduces our hierarchical formulation. Section III includes a numerical comparison with an analytical solution in a simplified setting. Section IV discusses the effect of noise in the state estimates and how it can be ameliorated. Section V considers the nonconvex collision avoidance constraints and their linearization. Finally, Section VI presents the experimental evaluation.

II. THE MINIMUM-TIME PROBLEM

A. A classical formulation

We consider a discrete-time linear dynamical system

x k+1 = Ax k + Bu k ,
to model the evolution of the joint angles q of a manipulator system, where x k ∈ R nx and u k ∈ R nu are the state variables and control inputs, respectively. The system matrices A ∈ R nx×nx and B ∈ R nx×nu could be arbitrary (however, we assume that the origin is reachable). In this paper we consider x k = (q k , qk ) and u k = qk , i.e., a double integrator. Note that the use of alternative dynamical systems might be beneficial, e.g., a triple integrator [START_REF] Zanasi | Discrete minimum time tracking problem for a chain of three integrators with bounded input[END_REF], [START_REF] Gerelli | A discrete-time filter for the on-line generation of trajectories with bounded velocity, acceleration, and jerk[END_REF]. Transferring a given initial state x (c) (at discrete sampling time c) to the origin in minimal time can be achieved by solving [START_REF] Van Den Broeck | A model predictive control approach for time optimal point-to-point motion control[END_REF], [START_REF] Borrelli | Predictive Control for linear and hybrid systems[END_REF] minimize

N subject to x k+1 = Ax k + Bu k x 0 = x (c) x N = 0 u k ∈ U k g(x 1 , . . . , x N ) ≥ 0 N ∈ {N min , . . . , N max }, (1)
with k = 0, . . . , N -1 and U k being a closed and bounded set containing zero in its interior (we assume it to be convex). The decision variables are x 1 , . . . , x N , u 0 , . . . , u N -1 and the number of discrete-sampling intervals N . Note that, by design, we are not interested in reaching the origin faster than N min sampling intervals in order to avoid aggressive behavior near the origin (as we will discuss in Section IV). g(x 1 , . . . , x N ) ≥ 0 includes collision avoidance constraints, which are in general nonconvex, as well as possibly other state related constraints, e.g., joint position and velocity limits. Note that this is a mixed integer programming problem. We will use N ⋆ c to denote the value of N at the solution of (1) (the subscript emphasizes the dependence on x (c) ).

B. A hierarchical formulation

The approach introduced in this paper is based on an equivalent reformulation of (1) as a hierarchical optimization problem: a standard multi-objective problem, where objectives can be assigned with different levels of priority. Hierarchical formulations are popular in robotics because they ensure that objectives with lower priority are optimized as far as they do not interfere with the optimization of objectives with higher priority [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF], [START_REF] Khatib | Whole-body dynamic behavior and control of human-like robots[END_REF].

Let us consider N max ≥ N ⋆ c , and define a sequence of states x = (x 1 , . . . , x N max ) and control inputs u = (u 0 , . . . , u N max -1 ). We introduce the following hierarchical problem lex minimize

x,u v = ( x N max 2 , . . . , x N min 2 ) subject to x k+1 = Ax k + Bu k x 0 = x (c) u ∈ U g(x) ≥ 0, (2)
with k = 0, . . . , N max -1 and U being a closed and bounded set containing zero in its interior. The "lex minimize " operator is standard and implies that the vector v is to be minimized according to lexicographic order [START_REF] Isermann | Linear lexicographic optimization[END_REF], that is, minimizing v i (in a least-squares sense) is infinitely more important than minimizing v j , for i < j. We will use P c to refer to (2) when we want to emphasize the dependence on the initial state x (c) .

The novelty of formulation ( 2) is in the particular choice of lexicographic objective. It states that the most important thing, after satisfying the constraints, is to reach the origin in N max number of sampling intervals. Then, if possible, try to reach the origin in N max -1 sampling intervals, and so on until N min intervals. This formulation ensures that each state x N min , . . . , x N max would be as close as possible to the origin (in Euclidean norm), and once the origin has been reached, the states x N ⋆ c , . . . , x N max would remain there. Note that we have chosen the origin as the target state only for convenience. An arbitrary target state can be used by a simple change of variable [START_REF] Khalil | Nonlinear Systems[END_REF]. Furthermore, if necessary, target regions can be considered by using a similar formulation.

As already discussed in the Introduction, problem (2) is nonconvex due to the collision avoidance constraints g(x) ≥ 0. We approach its solution by adopting an SQP type of scheme in an MPC context. That is, problem (2) with linearized collision avoidance constraints is solved during each control sampling interval c and only u (c) = u ⋆ 0 is applied to propagate the state from x (c) to x (c+1) (more details on the linearization are provided in Section V). Note that by a proper choice of N min and N max (which will be discussed Section IV) one can ensure timeoptimality for the linearized sub-problem. Each sub-problem can be integrated seamlessly in existing hierarchical control frameworks in robotics. Furthermore, its solution can be approached using already existing tools [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF], [START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF].

III. COMPARISON WITH THE ANALYTICAL SOLUTION FOR A DOUBLE INTEGRATOR

In special cases the minimum-time problem for discretetime linear dynamical systems subject to linear constraints has an analytical solution. One such case is when using a double integrator subject to simple bounds on the accelerations [START_REF] Gao | On discrete time optimal control: a closed-form solution[END_REF], [START_REF] Zanasi | Nonlinear filters for the generation of smooth trajectories[END_REF]. Here, numerical results from our hierarchical formulation are compared to this analytical solution. The purpose of this comparison is not so much to demonstrate the equivalence (which should be apparent from the analysis in Section II-B) but to emphasize the potential advantages of using numerical techniques for approaching the solution of the minimum-time problem. We consider the joint-space behavior of a two Degree of Freedom (DoF) manipulator and omit the constraint g(x) ≥ 0.

Let the optimal policy from [START_REF] Zanasi | Nonlinear filters for the generation of smooth trajectories[END_REF] be denoted by u ⋆ k = π(x k ). Using this policy is attractive because: (i) for any given state x k it gives us the control actions that ensure timeoptimal transition towards the goal and (ii) evaluating π(x k ) is computationally very cheap. Assuming that x 0 = x (c) and N max ≥ N ⋆ c , the recursion Fig. 4: Evolution of the control inputs for trajectory number 6 in Fig. 3. Blue and red correspond to formulation (4) and heuristics (5), respectively.

x k+1 = Ax k + Bπ(x k ), k = 0, . . . , N max -1 (3)
would reach the origin in minimal time (while taking into account the simple bounds on the controls) and remain there.

The optimal sequence of control actions

u ⋆ = (u ⋆ 0 , . . . , u ⋆ N max -1 ) generated from (3) coincides with the solution of lex minimize x,u v subject to x k+1 = Ax k + Bu k x 0 = x (c) u ∈ U, (4)
for an appropriately chosen U.

Note that, in the above setting, the simple bounds on u essentially decouple the joint motions. In our envisioned Fig. 5: Duration of each of the eleven trajectories in Fig. 3.

The minimal time as computed using ( 4) is depicted in blue, while the time required when using the heuristics ( 5) is depicted in red (a more than two times difference can be observed).

scenario, however, we expect to have additional linear constraints coupling the motion of the links (e.g., due to the linearization of the collision avoidance constraints). One possible option for still using the analytical solution would be to find u ⋆ k by solving minimize

u k u k -π(x k ) 2 subject to u k ∈ U k . (5)
The motivation behind ( 5) is to stay as close as possible (in Euclidean norm) to π(x k ) while respecting the additional constraints defined by U k . In order to evaluate the performance of (5) we compare it to (4) on a simple example, with more restrictive constraints on u k that couple the motion of the joints. Figure 2 depicts these constraints as a gray diamond (contained in the box defined by the simple bounds). Figure 3 depicts in blue eleven minimum-time joint-space trajectories converging to the origin generated using (4). The effect of using heuristics (5) for the 6-th trajectory can be seen in red (the corresponding control inputs are given in Fig. 4). For all trajectories we have used N min = 1 and N max = 29 ≥ N ⋆ i , i = 1, . . . , 11 (e.g., N ⋆ 6 = 25), with a control sampling time ∆t = 0.1 s. The duration of each trajectory is depicted in Fig. 5. As can be seen, using the heuristics (5) may result in more than twice slower transitions.

Based on these results we could conclude that even small modification of the constraints may render the analytical solution unsatisfactory. Since finding an analytical solution for arbitrary linear constraints is not straightforward it is beneficial to consider the numerical approach introduced here. Fig. 6: Duration of trajectory number 6 in Fig. 3 for varying N max . Similar pattern can be observed across all trajectories. Note that N ⋆ 6 = 25, hence even though in theory timeoptimality is guaranteed only for N max ≥ N ⋆ 6 , it appears that in practice satisfactory results can be obtained with much smaller N max .

IV. CHOOSING N min AND N max Formulation (2) involves the parameters N min and N max which should be specified by the user. The choice of N max reflects the length of the preview horizon and thus can be used to influence the reactivity of the system to dynamic obstacles. If it satisfies N max ≥ N = max(N ⋆ 1 , N ⋆ 2 , . . . ), time-optimality would be guaranteed withing each SQP subproblem. Even though N is not known beforehand, a reasonable guess for an upper bound can be made based on the particular industrial setting (e.g., by considering factors like types of obstacles, sampling time, velocity and acceleration limits). Note, however, that N max should not be chosen to be too large as it directly impacts the size of the problem to be solved.

Figure 6 depicts the influence of N max on the duration of joint-space trajectory number 6 from Fig. 3 (for which N ⋆ 6 = 25 with corresponding time of 2.5 s). As can be seen, in this particular case, time-optimality is achieved even for values considerably smaller than N ⋆ 6 . Even N max ∈ [7, 8, 9] appears to be acceptable, as the impact on the trajectory duration is rather small. We have observed that such behavior is very common even when additional state constraints are considered.

The choice of N min has an impact on the behavior of (2) in the vicinity of the setpoint when state measurement noise is present. On one hand, using N min = 1 results in a rather aggressive controller that always attempt at reaching the setpoint in one step. In the presence of noise this would result in high frequency chattering in the control signal. On the other hand, a too high value for N min might have a significant impact on the time optimal behavior. Finding a proper trade-off has been considered as an important problem [START_REF] Zanotto | Experimental validation of minimum time-jerk algorithms for industrial robots[END_REF], [START_REF] Borrelli | Predictive Control for linear and hybrid systems[END_REF]. Note that when the setpoint can be reached in m sampling intervals, using N min > m leads to redundancy (the solution of ( 2) is not unique) which can be exploited to optimize additional criteria (that can be used to formulate a desired trade-off). This can be achieved by simply adding more hierarchical levels to [START_REF] Andreasson | Autonomous transport vehicles: where we are and what is missing[END_REF].

Figure 7 depicts the influence of N min on joint-space trajectory number 6 from Fig. 3, when the state measurement is corrupted by Gaussian noise with zero mean and standard deviation 0.005. The objective of ( 4) is modified to lex minimize ( x N max 2 , . . . , x N min 2 , u 2 ), i.e., an additional optimization criterion is introduced.

The blue and red curves represent cases with N min = 6 and N min = 1, respectively. The top plot illustrates the profile of the control input of joint 1. As can be seen, the minimization of u 2 has a filtering effect on the high frequency chattering (which is desirable in practice). The lower plot depicts the resultant profiles of the angle of joint 1: they are hardly distinguishable. This implies that a proper choice of N min can have a smoothing effect on the control profiles without degrading the time-optimal behavior significantly.

In summary, the parameters N min and N max can be used to achieve a trade-off between time-optimality, problem size and smoothness of the solution (in the vicinity of the setpoint).

V. COLLISION AVOIDANCE CONSTRAINTS

Collision avoidance constraints g(x) ≥ 0 can be defined in terms of various primitive shapes [START_REF] Sugiura | Real-time collision avoidance with whole body motion control for humanoid robots[END_REF], [START_REF] Flacco | A depth space approach to human-robot collision avoidance[END_REF]. We consider a standard model that approximates the shape of the robot and the obstacles using a composition of spheres and swept sphere lines [START_REF] Sugiura | Real-time collision avoidance with whole body motion control for humanoid robots[END_REF]. Due to the nature of the envisioned application, the collision avoidance constraints are dynamically changing i.e., not known in advance, and are moreover nonconvex. The MPC scheme that we have adopted here can be used to address both issues. Not only it increases the reactivity of the controller but also it can be used to develop accurate local linear approximations of g(x) ≥ 0. This last point is precised next.

For clarity, first we consider collision avoidance constraints between a given link of the manipulator and a static circular obstacle. Suppose that the obstacle is centered at position h ∈ R 3 . Let p (c) k be the point on the link that is closest to the obstacle during the k-th sampling interval of the preview associated with P c . Then, in order to avoid collision, the Euclidean distance between p (c) k and h:

d (c) k = a (c) k • p (c) k -h , a (c) k = p (c) k -h p (c) k -h
, must remain greater than a minimal safety distance d s :

d (c) k ≥ d s . (6)
This is a nonconvex constraint and accounting for it explicitly can be computationally costly. That is why, we approximate it by observing that P c is closely related to P c-1 . This fact is heavily used in the field of predictive control not only to formulate simple and expressive constraints but to warm-start each optimization process with an adequate initial guess [START_REF] Wang | Fast model predictive control using online optimization[END_REF]. Following the exposition in [START_REF] Schulman | Motion planning with sequential convex optimization and convex collision checking[END_REF], we use an approximation:

a (c) k ≈ p (c-1) k-1 -h p (c-1) k-1 -h , p (c) k ≈ p (c-1) k-1 + J (c-1) k-1 q(c) k ,
where J (c-1) k-1 is the Jacobian matrix associated with p (c-1) k-1 . This way, the constraint (6) can be approximated using

a (c) k • J (c-1) k-1 q(c) k + p (c-1) k-1 ≥ d s , (7)
which is linear in q(c) k (a part of the decision variables of P c ). Alternatively one can use

q(c) k = q (c) k -q (c-1) k-1 ∆t ,
with ∆t being the sampling time, to reformulate [START_REF] Bertsekas | Dynamic Programming and Optimal Control[END_REF] in terms of q (c) k . Approximating g(x) ≥ 0 by using linear constraints like [START_REF] Bertsekas | Dynamic Programming and Optimal Control[END_REF] for each link of the manipulator for k = 1, . . . , N max , renders problem (2) with only linear constraints and a lexicographic least-squares objective, which is a class of problems commonly solved in robotics.

The only modification needed in case of a dynamic obstacle, assuming that its position over the preview horizon is known, is that one has to consider a time-varying h in the above derivations. Using other primitive shapes instead of a sphere to model obstacles is readily possible (this would only alter how the closest point is computed [START_REF] Ericson | Real-Time Collision Detection[END_REF]). Note that generating collision avoidance constraints between a given manipulator link and all present obstacles is not necessary, as indicated by state of the art collision detection approaches.

VI. EXPERIMENTAL VERIFICATION

We consider the industrial setup in Fig. 1. Two Adept Cobra s600 SCARA robots are performing pick and place operations while sharing the same working environment. Each manipulator has a dedicated controller and considers the other manipulator as a dynamic obstacle. Snapshots from a typical operation are depicted in Fig. 8.

In such industrial applications, the typical approach is for a programmer to specify intermediate paths between a large number of endpoints (for each robot). On these paths, acceleration profiles must then be defined. This process requires a lot of experience and takes considerable time and effort which prevents many manufacturers from using multi-robot systems. In contrast, the approach proposed here requires simply the desired endpoints to be specified by the user, while the intermediate trajectory is generated online.

In the experiment presented here, in addition to bounding the joint accelerations, constraints on the joint angles and velocities were imposed (see Table I). The underlying optimization problem for each manipulator was solved on a PowerPC CPU of 400 MHz under the constraint that not more than half of the CPU power can be utilized (32 ms control sampling time was used). This poses a challenge to our numerical approach (the hierarchical problems were solved using an implementation of the method in [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF]).

Collision avoidance constraints were formed by considering each link of one of the manipulators (modeled using a swept sphere lines) as an obstacle for the other. We followed the linearization procedure described in Section V. Since there was no notable state estimation noise, N min = 1 was used. Although we were aiming at having a preview length N max = 7 (which was verified to lead to very satisfactory results in a simulation study), due to the hardware limitations, N max = 5 was considered.

Figure 9 (a) depicts typical joint profiles of one pick and place cycle from the experiment (for one of the robots).

The actual experiment can be seen in the accompanying video. The results demonstrate that online generation of fast manipulator motions with the proposed hierarchical approach is readily possible even with limited resources. Although our choice of N max makes online computations feasible it, however, leads to an undesired "velocity saturation". Note how the velocity of joint 2 saturates at approximately 400 deg/s during the interval [0.25, 0.5]. This is a good indicator that by increasing N max one can expect to achieve faster transitions. The results with N max = 7 (obtained in a simulation) confirm this. As can be seen on Fig 9 (b), the velocities of both joints are very close to the actual limits and, in our experience, increasing further N max leads to only a marginal gain. The resultant transition duration is 30% faster compared to the case with N min = 5. Our current efforts are in the direction of reducing this gap by means of improving our numerical tools so that a larger N max can be used or by enhancing our formulation. For example, we are investigating the effects of non-uniform sampling of the preview window and alternative warm-starting techniques.

In order to emphasize the online generation of the trajectories, the accompanying video includes a variant of the above industrial setup where the targets are moving on conveyors.

VII. CONCLUSION

This paper introduced a hierarchical approach to minimum-time control. It is applied in the context of online trajectory generation for industrial robots performing pick and place operations in the presence of dynamic obstacles. In particular, we presented experimental evaluation using two SCARA robots that share the same working environment. The proposed formulation simplifies greatly the deployment of industrial technology, as it does not rely on the tedious and time consuming task of manually specify paths between a large number of endpoints. We achieve a reactive behavior by using model predictive control, and our approach has the capacity to suppress high frequency chattering in the control signal in the presence of noise: a common drawback of aggressive control strategies. An important advantage of our hierarchical formulation is that the solution of the underlying problem can be approached using already existing tools in robotics.

1 2 ]Fig. 2 :

 22 Fig. 2: Coupling constraint (diamond) and box constraints with dashed line. Blue and red dots depict the control profiles from Fig. 4.

Fig. 3 :

 3 Fig.3: Eleven joint-space trajectories. Each trajectory starts with zero joint velocity and converges to the origin.

2 ]Fig. 7 :

 27 Fig.7: Test with joint-space trajectory 6 from Fig.3when the state measurement is corrupted by Gaussian noise with zero mean and standard deviation 0.005. The blue and red curves represent cases with N min = 6 and N min = 1, respectively (N max = 29).

Fig. 8 :Fig. 9 :

 89 Fig. 8: Snapshots from a typical pick and place operation (a) → (b) → (c) → (d).

  ). Profiles of joints 1 and 2 are depicted using red and blue, respectively.

			joint 1	joint 2
	angle	[deg ]	[-105, 105]	[-150, 150]
	velocity	[deg/s]	[-322, 322]	[-600, 600]
	acceleration [deg /s 2 ]	[-2000, 2000]	[-3000, 3000]

TABLE I :

 I Bounds on joint angles, velocities and accelerations.

For clarity of presentation, and due to computational restrictions, we consider only one such sub-problem per control sampling time, even though being able to solve multiple sub-problems may lead to improved results.


Diana Serra
email: diana.serra@unina.it

Camille Brasseur

Alexander Sherikov

Dimitar Dimitrov

Pierre-Brice Wieber
email: pierre-brice.wieber@inria.fr

A Newton method with always feasible iterates for Nonlinear Model Predictive Control of walking in a multi-contact situation

In this paper, we present a Nonlinear Model Predictive Control scheme, which is able to generate walking motions in multi-contact situations. Walking up and down stairs with an additional hand support is a typical example, which we address in simulation. Computing such a nonlinear control scheme is usually done with a Newton method, a potentially time-consuming procedure involving iterative linearizations. We propose here a Newton method which is specifically designed to provide at each iteration a feasible solution, always satisfying the (nonlinear) dynamic balance constraints. This results in a significant reduction in computation time, by minimizing the number of necessary iterations to reach a feasible solution.

I. INTRODUCTION

The main difficulty in legged locomotion is the dynamic constraints on the linear and angular momentum of the robot, due to its dependance on external contact forces to control them [START_REF] Wieber | Modeling and control of legged robots[END_REF]. This is particularly evident in the Newton and Euler equations of motion of the robot, which can typically be expressed in the following form:

m(c + g) mc × (c + g) + L ∈ C, (1)
where m ∈ R is the mass of the robot, c ∈ R 3 is the position of its Center of Mass (CoM) in a world frame, L ∈ R 3 is its angular momentum with respect to the CoM, -g ∈ R 3 is the acceleration due to gravity, and C ⊂ R 6 is the cone of available contact wrenches (forces and torques), which depends on the existing contacts between the robot and its environment.

The standard approach to computing, and controlling motions satisfying such contraints is through Model Predictive Control (MPC) [START_REF] Wieber | Modeling and control of legged robots[END_REF], [START_REF] Mayne | Constrained model predictive control: Stability and optimality[END_REF]. When walking on a flat, or moderately uneven ground, as long as hand contacts are not necessary, the constraint (1) can take a linear form under classical assumptions [START_REF] Brasseur | A robust linear MPC approach to online generation of 3D biped walking motion[END_REF], and very efficient numerical methods can be employed, to compute the corresponding MPC scheme in a fraction of milli-second.

Hand contacts with the environment can be important however, to improve stability, energetic efficiency, to interact with the environment, or to simply realize otherwise infeasible motions [START_REF] Harada | Dynamical balance of a humanoid robot grasping an environment[END_REF], [START_REF] Koyanagi | A pattern generator of humanoid robots walking on a rough terrain using a handrail[END_REF], [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF], [START_REF] Kudruss | Optimal control for whole-body motion generation using center-of-mass dynamics for predefined multi-contact configurations[END_REF], [START_REF] Agravante | Human-Humanoid Collaborative Carrying[END_REF]. In this case, the constraint [START_REF] Wieber | Modeling and control of legged robots[END_REF] is generally nonlinear, and we have to resort to a Newton method to obtain a solution, a potentially time-consuming and uncertain procedure involving iterative linearizations.

In an MPC scheme, we have to solve a sequence of closely related optimization problems, one at each sampling time. Using tools from parametric optimization and continuation methods, it is possible to obtain very good approximate solutions to these nonlinear problems, with very few, or even only one iteration at each sampling time [START_REF] Diehl | Real-time optimization for large scale nonlinear processes[END_REF]. The problem, however, is that these approximate solutions may not always be feasible and satisfy exactly the nonlinear constraints. This is the problem that we tackle in this paper, by proposing a Newton method based on carefully crafted linear approximate models, which ensure that the nonlinear constraint (1) is always satisfied exactly. Our approach is based on a linear approximation which is made robust to polytopic uncertainties, following the same procedure as in [START_REF] Brasseur | A robust linear MPC approach to online generation of 3D biped walking motion[END_REF], and constraining each iteration of the Newton method to fall within the limits of the specified polytopic uncertainty. A specific form of the constraint (1) is used, where a Center of Pressure (CoP) is introduced for foot contacts on the ground, while other external forces are considered as a generic force f e ∈ R 3 and torque n e ∈ R 3 acting on the CoM, as proposed in [START_REF] Agravante | Human-Humanoid Collaborative Carrying[END_REF]. Simulation results show that we can very efficiently compute walking motions up and down stairs with an additional hand support, which satisfy exactly the balance constraint (1). This paper is organized as follows. Section II presents the proposed MPC scheme for generating walking motions with additional hand support. The dynamic model of the legged robot, the kinematic and dynamic feasibility constraints for balance, and the objective function of the Optimal Control Problem (OCP) are presented. Section III presents our proposition for making sure that every iteration of our Newton scheme is always feasible, satisfying exactly the nonlinear constraint [START_REF] Wieber | Modeling and control of legged robots[END_REF]. Section IV finally presents an evaluation of the proposed scheme in the case of a walking motion up and down stairs, with statistics on the computational efficiency of the approach along with a comparison with other state of the art methods.

II. MODEL PREDICTIVE CONTROL OF WALKING A. Introducing the CoP

Let's consider the contact forces f i ∈ R 3 between the feet and the ground separately from the rest of external forces and torques in the Newton and Euler equations of motion, as proposed in [START_REF] Agravante | Human-Humanoid Collaborative Carrying[END_REF]:

m(c + g) = f e + i f i , (2)
mc × (c + g) + L = n e + c × f e + i s i × f i , (3)
where s i ∈ R 3 are the positions of the contact points between the feet and the ground, and everything else is as in (1). In the following, superscripts x, y and z indicate the corresponding components of a vector.

In the proposed approach, dynamic feasibility can be safely checked every 100 ms only, so with short enough double support phases, we end up checking dynamic feasibility only during single support phases [START_REF] Herdt | Online walking motion generation with automatic footstep placement[END_REF]. In that case, we can safely assume that every time we check dynamic feasibility, all contact points between the feet and the ground actually have the same height, s z i = s z for all i, even when walking up and down stairs, and gravity is orthogonal to the ground, g x = g y = 0.

Dividing the Euler equation by the z component of the Newton equation, we obtain

mc × (c + g) -c × f e -n e + L m(c z + g z ) -f z e = i s i × f i i f z i . (4)
Focusing on the x and y components of this equation, we can introduce the CoP

p = i s xy i f z i i f z i = c xy - (c z -s z )(mc xy -f xy e ) m(c z + g z ) -f z e - Ω ñxy m(c z + g z ) -f z e , (5)
where ñ = L -n e = i (s i -c) × f i is the torque exerted by the contact forces f i with respect to the CoM, and

Ω = 0 1 -1 0 .
Note that the CoP is a linear function of the x and y components of motion (c xy , cxy , f xy e , ñxy ), and a nonlinear function of the z components (c z , cz , f z e ).

B. Constraints

Since the contact forces with the ground are usually unilateral, f z i ≥ 0 for all i, the CoP must lie within the convex hull of the contact points s xy i :

p ∈ S(s xy i ). (6)
The maximal reachable region for the CoM with respect to the center of the support foot s f ∈ R 3 , is typically approximated by a convex polytope [START_REF] Brasseur | A robust linear MPC approach to online generation of 3D biped walking motion[END_REF]:

A(c -s f ) ≤ b. (7)
Finally, external forces and torques are also bounded. We follow here the approach proposed in [START_REF] Kudruss | Optimal control for whole-body motion generation using center-of-mass dynamics for predefined multi-contact configurations[END_REF] and consider constraints:

f e ≤ f e ≤ f e , ñ ≤ ñ ≤ ñ. (8)

C. Objectives

Our first objective is to keep the height of the CoM as close as possible to a reference cz s ∈ R chosen above the kinematic limit [START_REF] Kudruss | Optimal control for whole-body motion generation using center-of-mass dynamics for predefined multi-contact configurations[END_REF],

o 1 = (c z -s z ) -cz s 2 . (9)
Then, robustness of the walking motion to perturbations is improved by minimizing also the deviation of the CoP from the center of the support foot s xy f ,

o 2 = p -s xy f 2 . ( 10
)
Finally, motion smoothness is improved by minimizing additionally the jerk of the CoM and the external wrench,

o 3 = ... c 2 , o 4 = f e 2 , o 5 = ñ 2 . ( 11
)
All these are standard objectives when generating walking motions.

D. Optimal Control Problem

In the end, to find trajectories of the CoM with the corresponding external wrench, an OCP is formulated on a finite time horizon T h : minimize ... c ,fe,ñ 6), [START_REF] Kudruss | Optimal control for whole-body motion generation using center-of-mass dynamics for predefined multi-contact configurations[END_REF], [START_REF] Agravante | Human-Humanoid Collaborative Carrying[END_REF], ∀t,

T h 0 5 i=1 w i o i dt subject to (
where o i are the objectives ( 9), [START_REF] Herdt | Online walking motion generation with automatic footstep placement[END_REF], and [START_REF] Kaneko | Humanoid robot hrp-4 -humanoid robotics platform with lightweight and slim body[END_REF], and w i are corresponding weights.

III. ALWAYS FEASIBLE NEWTON ITERATES

The following Section presents our proposition for making sure that every iteration of the Newton scheme always satisfies exactly the nonlinear constraint [START_REF] Wieber | Modeling and control of legged robots[END_REF].

A. Robustness to polytopic uncertainties

Observing that equation ( 5) is linear with respect to the x and y components of motion, and nonlinear with respect to its z component, walking motions were generated in [START_REF] Brasseur | A robust linear MPC approach to online generation of 3D biped walking motion[END_REF] by adopting a Linear MPC scheme on the x and y components, that was made robust to the nonlinearity on the z component, bounded as a polytopic uncertainty. We propose to follow a similar approach here, with the additional capacity to use hand support.

Equation ( 5) can be reformulated as follows,

p(ζ 1 , ζ 2 ) = c xy -ζ 1 cxy - f xy e m + ζ 2 Ω ñxy , (13)
by introducing

ζ 1 = m(c z -s z ) m(c z + g z ) -f z e , ζ 2 = 1 m(c z + g z ) -f z e . (14)
Note that this equation is linear with respect to ζ 1 and ζ 2 . Let's consider now that these variables stay between some bounds:

0 ≤ ζ 1 ≤ ζ 1 ≤ ζ 1 , 0 ≤ ζ 2 ≤ ζ 2 ≤ ζ 2 . ( 15
)
If we make sure that the constraint ( 6) is satisfied for all extreme values of ζ 1 and ζ 2 , we can conclude that it is satisfied for all values in between, by a simple convexity argument:

{p(ζ 1 , ζ 2 ), p(ζ 1 , ζ 2 ), p(ζ 1 , ζ 2 ), p(ζ 1 , ζ 2 )} ⊂ S(s xy i ) (16) ⇓ p(ζ 1 , ζ 2 ) ∈ S(s xy i ).
Since the constraint ( 16) is defined with fixed, extreme values of ζ 1 and ζ 2 , we can see from ( 13) that it doesn't involve anymore the z components of motion, and is simply linear with respect to its x and y components.

On the other hand, the combination of ( 14) and ( 15) imposes linear constraints on the z components of motion:

ζ 1 (m(c z + g z ) -f z e ) ≤m(c z -s z ) ≤ ζ 1 (m(c z + g z ) -f z e ), (17)
ζ -1 2 ≤m(c z + g z ) -f z e ≤ ζ -1 2 . (18)
This way, if we make sure that the x and y components of motion satisfy the linear constraint (16) while the z components satisfy the linear constraints ( 17) and ( 18), we can conclude that the 3D motion satisfies the nonlinear constraint [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF].

B. Constraining Newton iterates

Each iteration of a Newton scheme involves computing the solution to a linear approximation of the nonlinear problem. The key to our approach is to constrain each of these iterations, to satisfy the linear constraints (16), ( 17) and [START_REF] Escande | Planning contact points for humanoid robots[END_REF], in order to make sure that the nonlinear constraint ( 6) is always satisfied exactly.

Instead of considering fixed bounds on ζ 1 and ζ 2 in [START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF], that would require to be finely tuned as in [START_REF] Brasseur | A robust linear MPC approach to online generation of 3D biped walking motion[END_REF], we propose here to adapt them at each iteration in the following way, based on the value of the parameters ζ 1 and ζ 2 at the previous iteration:

ζ (j) 1 = ζ (j-1) 1 -kµ 1 , ζ (j) 1 = ζ (j-1) 1 + kµ 1 , (19)
ζ (j) 2 = ζ (j-1) 2 -kµ 2 , ζ (j) 2 = ζ (j-1) 2 + kµ 2 , (20)
with positive constants k, µ 1 and µ 2 .

IV. SIMULATION RESULTS

The proposed approach is evaluated on a walking motion up and down stairs, with an external force generated by hand supports at two predefined time intervals, as shown on Figure 1. This motion is generated for a simulated HRP-4 [START_REF] Kaneko | Humanoid robot hrp-4 -humanoid robotics platform with lightweight and slim body[END_REF], using the parameters provided in Table I. A. Generated motion

We can see in Figure 2 how the presence of an additional hand support during the steps up and down stairs affects the motion of the CoM in the frontal plane (y, z). The weighted sum of objectives o 1 , o 2 and o 3 naturally decreases when the external wrench is employed, since it represents an additional degree of freedom for the OCP. Figure 3 shows the corresponding evolution of the norm of the external force f e and torque ñ during the two predefined moments. Since a minimization of the norm of the wrench is performed, higher weights, w 4 and w 5 , on f e and ñ generate lower values of the resultant wrench. We use a higher weight for the external torque (w 5 > w 4 ) in order to reduce the rotational motion around the CoM. The proposed weights w i are given in Table II.

We can see in Figures 4 and5 how the CoP and the CoM converge over iterations of the Newton scheme for a given preview horizon. We can observe that the first iteration already provides a very good approximate solution, only a few millimeters away from the optimum. More importantly, the method we propose makes sure that it satisfies exactly the nonlinear constraint [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF]. 16), while all the extreme points are kept inside the support polygon S(s xy i ) by the corresponding constraints. Four different computations are shown, with a range of polytopic uncertainty {µ 1 , µ 2 } multiplied by four different coefficients, k ∈ {1, 0.6, 0.4, 0.2}. We can see how the choice of a larger range leads to much more conservative constraints. Figure 7 shows how this more conservative constraint affects the value that can be reached for the objective function. It appears clearly that a smaller range leads to better results.

B. Nonlinear constraint and polytopic uncertainty

Note that the range of values for the polytopic uncertainties was chosen empirically, as inappropriate values can quickly lead to infeasible problems, what would require a relaxation of constraints.

C. Computational efficiency

Solutions are computed with qpOASES [START_REF] Ferreau | qpOASES: A parametric active-set algorithm for quadratic programming[END_REF]. Without using hotstart, on a relatively slow Intel Core i5-4200U @ 1.60GHz, the solutions are obtained in approximately 8 ms (average time over the whole trajectory generation). This result can certainly be improved significantly with hotstart. As a comparison, the computation time reported for a similar problem in [START_REF] Carpentier | A versatile and efficient pattern generator for generalized legged locomotion[END_REF] is 5.5 s for 85 iterations, which gives approximately 65 ms for each iteration, on a much faster CPU. Not only is our Newton scheme much safer, by providing iterates which are always feasible, it appears also to be an order of magnitude more efficient in terms of computation time.

D. Whole body motion

Simulations of the resulting whole body motion are performed (see Figure 8), using a standard inverse dynamics approach [START_REF] Fujimoto | Proposal of biped walking control based on robust hybrid position/force control[END_REF], [START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF]. Our whole body motion controller employs PD controllers to track the reference trajectory of the CoM produced by the MPC scheme, and the trajectories of the feet and hands generated using cubic polynomials. Positions of the hands are controlled only during the short intervals when they are approaching the contact points -during the rest of the simulation the reference configuration of the arms is maintained with PD controllers instead. The whole body motion controller obeys contact friction constraints and constraints due to dynamics and kinematics of the robot. These constraints include the joint torque limits, which, however, are never reached in our simulations.

These simulations are illustrated in the accompanying video [16], which consists of four parts: the first two illustrate walking on a flat ground, while the last two demonstrate walking up and down stairs. Due to the fact that the reference external wrench acts directly on the CoM, we have a relative freedom in choosing the contact configurations. In order to demonstrate this we generate the reference external wrench using a single hand contact in the first and third parts of the video, while in the second and fourth parts we produce exactly the same external wrenches using two hand contacts.

In the case of walking on a flat ground, tuning of the MPC and whole body motion controller amounted to finding kinematically feasible contact point positions, which can be relatively easily parameterized with the CoM position. We experienced more difficulties while working on walking on stairs. Most of them were related to infeasibility of the reference trajectories due to the mechanical limits of the knee (going up) and ankle (going down) joints. The latter appears to be a particularly severe limitation, which makes walking downstairs with flat feet a much more challenging task than going upstairs. We alleviated the issues with the joint limits by tuning the swing foot trajectories and the constraint on the maximal distance between the feet and the CoM. In addition, we considered relatively low stairs when going downstairs (8 cm), instead of much higher ones when going upstairs (16 cm).

V. CONCLUSION

We have proposed a Newton method which provides feasible iterates, always satisfying the nonlinear constraint [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF], for a Nonlinear MPC scheme generating walking motions in multi-contact situations. It is based on a linear approximation which is made robust to polytopic uncertainties, constraining each iteration of the Newton method to fall within the limits of these polytopic uncertainties. The simulation of a walking motion up and down stairs with additional hand support demonstrates the applicability of our method, with remarkable computational efficiency. This approach works as well for different multi-contact situations, since the generated external wrench is not dependent on the specific contact configuration.

The proposed method relies on predefined footsteps and hand supports. Automatic footstep placement on horizontal surfaces could be included easily using the methods introduced in [START_REF] Herdt | Online walking motion generation with automatic footstep placement[END_REF], but more complex cases of uneven ground and multi-contact situations require much more advanced planning methods [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF], [START_REF] Deits | Footstep planning on uneven terrain with mixed-integer convex optimization[END_REF], [START_REF] Escande | Planning contact points for humanoid robots[END_REF], [START_REF] Tonneau | A reachability-based planner for sequences of acyclic contacts in cluttered environments[END_REF]. Our next steps however are to validate the proposed approach on a real robot, and to evaluate how a lexicographic approach could be introduced, to automatically decide when it is necessary to use the additional hand support, as in [START_REF] Sherikov | Balancing a humanoid robot with a prioritized contact force distribution[END_REF].

Fig. 1 . 2 Fig. 2 .Fig. 3 .

 1223 Fig.1. An example of walking motion climbing up and down stairs, generated online with the proposed Nonlinear MPC scheme. Footprints are represented with yellow rectangles, while their sequence is indicated with a red broken line. The trajectory of the CoM is indicated in blue, and the corresponding external force appears as black arrows.

4 Fig. 4 .

 44 Fig. 4. Semi-log graph showing the convergence of the CoP trajectory over iterations of the Newton scheme.

4 Fig. 5 .

 45 Fig. 5. Semi-log graph showing the convergence of the CoM trajectory over iterations of the Newton scheme.

Figure 6

 6 Figure 6 shows how the CoP always stays inside the quadrilateral defined by the four extreme points p(ζ 1 , ζ 2 ), p(ζ 1 , ζ 2 ), p(ζ 1 , ζ 2 ) and p(ζ 1 , ζ 2 ) due to argument (16), while all the extreme points are kept inside the support polygon S(s xy i ) by the corresponding constraints. Four different computations are shown, with a range of polytopic uncertainty {µ 1 , µ 2 } multiplied by four different coefficients, k ∈ {1, 0.6, 0.4, 0.2}. We can see how the choice of a larger range leads to much more conservative constraints. Figure7shows how this more conservative constraint affects the value that can be reached for the objective function. It appears clearly that a smaller range leads to better results.Note that the range of values for the polytopic uncertainties was chosen empirically, as inappropriate values can quickly lead to infeasible problems, what would require a relaxation of constraints.

Fig. 6 .Fig. 7 .Fig. 8 .

 678 Fig. 6. The trajectory of the CoP (blue line) is always kept inside the quadrilateral defined by the four extreme points p(ζ 1 , ζ 2 ), p(ζ 1 , ζ 2 ), p(ζ 1 , ζ 2 ) and p(ζ 1 , ζ 2 ), which are all kept inside the support polygon S(s xy i ) (black rectangle). The range of polytopic uncertainty µ 1 and µ 2 is multiplied here by four different coefficients, from top to bottom: k = 1, k = 0.6, k = 0.4, k = 0.2.

ACKNOWLEDGMENTS

This work has been funded by the PSPC Romeo 2 project, the EU H2020 Comanoid Research and Innovation Action (RIA) and the RoDyMan project, which has received funding from the European Research Council FP7 Ideas under Advanced Grant agreement number 320992.