Néstor Bohórquez

Alexander Sherikov

Dimitar Dimitrov

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

Safe navigation strategies for a biped robot walking in a crowd

Safety needs to be guaranteed before we can introduce robots into our working environments. For a biped robot to navigate safely in a crowd it must maintain balance and avoid collisions. In highly dynamic and unpredictable environments like crowds, collision avoidance is usually interpreted as passive safety, i.e. that the robot can stop before any collision occurs. We show that both balance preservation and passive safety can be analyzed, from the point of view of viability theory, as the ability of the robot to stop safely at some point in the future. This allows us to address both problems with a single model predictive controller with appropriate terminal constraints. We demonstrate that this controller predicts failures (falls and collisions) as early as the duration of the preview horizon. Finally, we propose a new strategy for safe navigation that relaxes the passive safety conditions to allow the robot to avoid a greater number of collisions.

I. INTRODUCTION

The objective in this paper is to make biped robots walk safely in a human crowd. This presents two particular problems: maintaining balance and avoiding collisions. It has already been observed that both problems can be addressed with the concept of viability [START_REF] Wieber | Viability and Predictive Control for Safe Locomotion[END_REF]. In viability theory [START_REF] Aubin | Viability theory[END_REF], the objective is to ensure that the system avoids undesirable states in the future. In our case, the undesirable states are those where the robot has fallen or collided with somebody. The difficulty resides in achieving these objectives with limited knowledge of the environment and of the future, specially of the future motion of people around the robot.

If we can make sure that the robot can stop in a few steps in a stable position (i.e. that it is capturable [START_REF] Pratt | Capture point: A step toward humanoid push recovery[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]), then it can keep its balance indefinitely. This can be accomplished with limited knowledge of the environment and it has proven to be compatible with the realization of highly dynamic motions [START_REF] Sherikov | Whole body motion controller with long-term balance constraints[END_REF]. The case of collision avoidance is more challenging, as we cannot guarantee it indefinitely, specially in a crowd with fast moving persons. We can ensure, however, passive safety [START_REF] Macek | Towards Safe Vehicle Navigation in Dynamic Urban Scenarios[END_REF], [START_REF] Bouraine | Provably Safe Navigation for Mobile Robots with Limited Field-of-Views in Dynamic Environments[END_REF], i.e. that the robot is able to stop before any collision occurs. This corresponds to the emergency stop procedures required for industrial and personal care robots [START_REF] Iso | Robots and robotic devices -Safety requirements for industrial robots -Part 1: Robots[END_REF], [START_REF]Robots and robotic devices -Safety requirements for personal care robots, ser[END_REF]. We can observe that the capacity to stop is integral to both concepts and, thanks to this, passive safety can be combined effortlessly with capturability. In our approach, we incorporate these two concepts in a single Model Predictive Control (MPC) scheme [START_REF] Mayne | Constrained model predictive control: Stability and optimality[END_REF].

There are situations where it is not possible for the robot to avoid a fall or a collision. It is fundamental to predict these situations as early as possible for the safety of the people around, of the robot and of the workspace [START_REF] Kalyanakrishnan | Learning to predict humanoid fall[END_REF], [START_REF] Hohn | Probabilistic balance monitoring for bipedal robots[END_REF]. By previewing the short-term future, our proposed controller predicts failures before they occur. We can make the anticipation time of such events arbitrarily early (by previewing further into the future) provided that the perception capabilities of the robot satisfy some minimal conditions related to the speed at which the environment evolves.

Lastly, we propose a new strategy for safe navigation in dynamic environments that builds upon the observation that the stop requirement in passive safety can limit the mobility of the robot (specially in close proximity of people). This behavior is accentuated as uncertainty in the perception of the environment increases. We demonstrate that it produces fewer collisions than passive safety and, when they happen, they are less severe.

We proceed as follows: we briefly describe the basis for our numerical approach to safe navigation in Section II. Then we introduce the elements of the problem of walking in a crowd in Section III. We formulate two MPC controllers for passive safety and two for relaxed passive safety in Section IV. We evaluate the performance of all strategies through numerical simulations whose results we present in Section V.

II. LEXICOGRAPHIC OPTIMIZATION FOR SAFE

NAVIGATION

To achieve passive safety, a biped robot has to maintain balance and be able to stop before any collision happens. Additionally, we also want the robot to follow a reference velocity. But these objectives might conflict and, in such cases, at least one of them has to be relaxed [START_REF] Maciejowski | Predictive Control Constraints[END_REF]. To do this, we express all objectives as linear inequalities on the state x of the robot

b ≤ Ax ≤ b, (1)
where equalities can be expressed with b = b. Relaxation can be done, then, by introducing a violation v as in

b ≤ Ax -v ≤ b, (2)
and minimizing v 2 . Note that the objective is satisfied if v = 0.

Deciding which objectives can be relaxed is problemdependent. In our case, avoiding collisions and maintaining balance are more important objectives than following a reference velocity. We can assign a priority to each objective and minimize their violations accordingly, that is, as a lexicographic least squares problem [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], [START_REF] Kanoun | Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task[END_REF]. In order to do this, we define P levels of priorities, each one described by a triplet (A i , b i , b i ), and we look to lex minimize

x,{vi} ( v 1 2 , . . . , v P 2 ) subject to    b 1 . . . b P    ≤    A 1 . . . A P    x -    v 1 . . . v P    ≤    b 1 . . . b P    . ( 3
)
Lexicographic optimization is a key aspect of our approach to safe navigation, as will be explained in the following sections.

III. MODELS OF THE ROBOT AND THE CROWD

In this section, we first discuss the models that we consider for the anticipation of motions of the robot and the crowd. We then formalize the safety criteria for navigation (dynamic and kinematic feasibility, collision avoidance and stopping conditions) in the context of these models.

A. Dynamic and kinematic feasibility

A biped robot is a complex, nonlinear, underactuated, hybrid dynamical system [START_REF] Wieber | Springer Handbook of Robotics[END_REF]. However, we can generate standard walking motions (i.e. on a flat ground with a constant height of the Center of Mass (CoM)) using a linear relation between the horizontal motion of the CoM and the Center of Pressure (CoP) on the ground:

c - h g c = p. (4)
In this equation, c ∈ R 2 and p ∈ R 2 are the horizontal positions of the CoM and CoP respectively, h is the height of the CoM and g is the norm of the gravity vector. We consider only this simple situation in this paper, although walking motion can be generated on uneven ground with varying height of the CoM as well using similar linear approaches [START_REF] Brasseur | A robust linear MPC approach to online generation of 3d biped walking motion[END_REF]. Due to the unilaterality of the contact forces with the ground, the CoP is always constrained to stay inside the support polygon [START_REF] Wieber | On the stability of walking systems[END_REF]:

p ∈ P(s), (5)
where P(s) is a convex set that is defined relative to the center of the foot on the ground s ∈ R 2 . In combination with (4), this limits the acceleration that the CoM can achieve.

We enforce a kinematic constraint related to the maximal leg length of the robot:

c -s ≤ L. (6)
Moreover, most biped robots cannot cross their legs when walking. Consequently, we restrict the position of the (j + 1) th footstep with respect to the j th to a region where this cannot happen:

s j+1 ∈ S(s j ), (7)
where S(s j ) is a convex set defined relative to s j . We state that the motion of a biped robot is dynamically and kinematically feasible if the following expression holds:

Feasibility ≡ (4) ∧ (5) ∧ (6) ∧ (7) (F )
We linearize the constraints ( 5), ( 6) and ( 7) in our implementation by making standard assumptions on the kinematics of the robot [START_REF] Herdt | Lmpc based online generation of more efficient walking motions[END_REF], [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF].

B. Model of the crowd and collision avoidance

Let us consider a crowd of M persons. In order to avoid collisions, we enforce a minimal separation distance between the robot and each person:

Collision avoidance ≡ c -m k ≥ d (C)
for all k ∈ {1, . . . , M }. In this equation, m k ∈ R 2 is the position of the k th person and d is the minimal separation distance.

Passive safety requires the robot to avoid collisions in the present and in the future, until it is able to stop. We need, therefore, a model to anticipate the motion of people. We make the following assumptions: people walk at constant velocity (at least during the short period of time before the robot is able to stop) so they do not try to avoid the robot; and collisions among people are disregarded.

We also take into account that the robot perception of the crowd has some degree of uncertainty. Consequently, the estimated position mk and velocity mk of each person have associated errors:

mk = m k -mk , mk = ṁk -mk , (8)
with respect to the real position m k and velocity ṁk . We respond to this uncertainty by adapting the minimal separation distance in a conservative way:

d(t) = d 0 + mk + mk t. (9)
Here, given the magnitude of the uncertainty in position mk , we consider that the real position m k lies in a circle of radius mk centered at the estimated position mk . Furthermore, given the magnitude of the uncertainty in velocity mk , the radius of this circle increases at a rate mk t as it moves with the estimated velocity mk . Figure 1 shows two examples of how uncertainty shapes our model of the future.

In our implementation, we linearize the constraint on collision avoidance (C) as follows:

ûT k (c -m k ) ≥ d, (10)
where

ûk = (c -m k ) c -m k ∈ R 2 (11)
is a unit normal vector that points from the center of the k th person to the CoM of the robot. An advantage of this approach is that, by being conservative, this linear approximation is always safe with respect to the nonlinear problem, as can be observed in Figure 2.

C. Stop condition

We can determine trivially from ( 4) and ( 5) that the robot has stopped when ċ = c = 0 and c = p ∈ P(s). It is also possible to identify situations where the robot has not stopped yet but can stop without having to make any further step, i.e. where it is 0-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], by introducing the following constraint on the Capture Point:

Stop ≡ c + h g ċ ∈ P(s) (S)
This will be our stop condition.

D. Walking objectives

In addition to these constraints on dynamic and kinematic feasibility, collision avoidance and stopping condition, we would also like the CoM to follow, as much as possible, a Hierarchy 1: Passive safety 1: Passive safety conditions:

• Feasibility: F i , ∀i ∈ I • Collision avoidance: C i , ∀i ∈ I • Stop at the end: S N 2: Objectives: O i , ∀i ∈ I
reference walking velocity ċref [START_REF] Herdt | Online Walking Motion Generation with Automatic Foot Step Placement[END_REF]. Furthermore, we are interested in keeping the CoP as close as possible to the center of the foot on the ground s to improve the robustness of the robot against perturbations [START_REF] Wieber | Trajectory free linear model predictive control for stable walking in the presence of strong perturbations[END_REF]. We refer to these as:

Objectives ≡ ċ = ċref , p = s. (O)

IV. SAFE NAVIGATION STRATEGIES

In this Section, we discuss three different ways we can integrate the constraints of the robot to formulate safe navigation controllers.

A. Passive safety: triggering an emergency stop

Given a horizon of N sampling times into the future I = {1, . . . , N }, preserving passive safety means that the robot must satisfy the stop condition (S) at the N th sampling time, what we note here S N . It must also satisfy the constraints on dynamic and kinematic feasibility (F ) and collision avoidance (C) at all time until then, i.e. F i and C i for all i ∈ I. Once these three conditions are met, the remaining degrees of freedom can be used to satisfy, as much as possible, the objectives (O), i.e. O i for all i ∈ I. We can formalize the hierarchy between the constraints and the objectives as the lexicographic least squares problem summarized in Hierarchy 1.

The solution to this lexicographic problem provides the robot with a trajectory to walk safely and stop after N sampling periods. In order to generate a steady walking motion, we implement a receding horizon strategy: the lexicographic problem is re-evaluated at each sampling time for a horizon of N sampling periods in the future, continuously postponing the moment when the robot would eventually stop.

If it is not possible to safely postpone the moment when the robot stops, it can simply follow the last safe plan and stop before any collision occurs. When this happens, we raise an alarm to indicate that the robot is initiating an emergency stop and that it is not safe to continue walking. This alarm can be used to warn the surrounding crowd of the risk of collision N sampling periods in the future.

B. Passive safety with deferrable emergency stop

During the emergency stop, the robot will steadily approach the moment planned for the stop and the horizon will shrink accordingly. It might be beneficial, however, to continue re-evaluating the situation to check if, based on We can go through a simple loop, at each new sampling period, to find the largest N (up to a nominal value) such that all feasibility, collision avoidance and stopping condition are satisfied.

C. Relaxed passive safety

As soon as the robot is unable to plan a new safe motion, passive safety requires that it initiates an emergency stop. But in case of uncertainty, our conservative approach (see [START_REF]Robots and robotic devices -Safety requirements for personal care robots, ser[END_REF] and Figure 1) might exaggerate the risk of collision, leading to inappropriate stops. Another strategy could be to satisfy all passive safety constraints for as long as possible and, when not possible, relax them in order to produce a solution that is as close as possible to passive safety. In this way, the robot tries to minimize the risk of collision instead of simply initiating an emergency stop.

Building on our lexicographic optimization approach, we propose a relaxation of constraints along the horizon: we try to satisfy first the constraints in the near present and then, if possible, those in the far future. In particular, we can try to satisfy C i and, if possible, C i+1 for all i ∈ {1, . . . , N }. Correspondingly, the stop requirement S N and the objectives would belong now to the lowest levels. Hierarchy 2 shows how we order these priorities in a single formulation. In this case, all dynamic and kinematic feasibility constraints appear at the highest priority level, but the same relaxation scheme could be applied as well, i.e. F i ≻ F i+1 for all i ∈ {1, . . . , N }. Hierarchy 3 shows how we order the temporal relaxation of both feasibility and collision avoidance. A similar approach can be used to generate time-optimal trajectories for manipulator robots [START_REF] Homsi | A hierarchical approach to minimum-time control of industrial robots[END_REF].

V. EVALUATION OF THE SAFE NAVIGATION STRATEGIES

We evaluate numerically the performance of the proposed safe navigation strategies by making the robot traverse a moving crowd (as shown in Figure 3) under different environmental conditions and parameters of the controller. Table I shows the parameters for the kinematics of our simulated robot, which is based on the HRP-2 [START_REF] Kaneko | Humanoid robot hrp-2[END_REF]. Tables II andIII show the parameters of the two scenarios of our numerical evaluations.

For each combination of size and speed along x (M, ṁx k ) we generate and store 100 different crowds that only differ in the initial positions {m k } and speeds along y { ṁy k } of the participants. The initial positions vary uniformly over an area of 10 × 8[m 2 ] while speeds along y follow a normal distribution N (0, 0.2).

During simulations, we test each configuration of the controller (H, R, mk , mk ) against each of the 100 crowds generated for each combination (M, ṁx k ). Simulations last 20[s] or until a failure occurs, i.e. when the state of the robot is infeasible or when it collides with a person.

We use the notion of a Field of View (FoV): the maximal distance around the robot at which it is capable of perceiving persons/objects. We do not consider occlusions in perception, making the robot aware of the current position and velocity of everybody within the FoV.

A. No uncertainty in the motion of the crowd

Figure 4 synthesizes the results obtained from all simulations with no uncertainty in the motion of the crowd. Plots on the left show the failure rate as a function of the radius of the FoV and for different walking speeds of the crowd. Plots on the right report the corresponding median anticipation time to failure, which is the elapsed time between the moment the earliest alarm was triggered and the moment the failure

[-0.2, 0.2] ms -1 Uncertainty mk 0 m mk 0 ms -1
happened. It is worth mentioning that, in these simulations with no uncertainty, all failures are anticipated as early as permitted by the limits of the FoV and the prediction horizon, and no false alarms are raised. We can see two different regimes represented with either dashed lines or solid lines. The threshold is attained when the radius R of the FoV reaches

R min = H( ṁx k + ċx ref ), (12)
which is the minimum value for detecting all people that might collide with the robot over a horizon of duration H, with walking speeds ṁx k for the people and ċx ref for the robot. If the FoV is smaller than this minimum value, the anticipation time for collisions grows linearly with the radius of the FoV and the failure rate decreases accordingly. It does not, however, decrease with longer prediction horizons: it appears that longer prediction horizons can degrade collision avoidance if the radius of the FoV is not large enough, as the robot is making decisions without proper knowledge of the surrounding crowd.

If the FoV is larger than this minimum value, the anticipation time for collisions reaches exactly the duration H of the prediction horizon, and the failure rate reaches a minimum, which does not vary further with the radius of the FoV. When the robot walks in an inattentive crowd that moves at a standard human walking speed (between 1[ms -1 ] and 1.5[ms -1 ] [25]), the probability of collisions is always higher than 50%. The robot does not have enough kinematic and dynamic capabilities to avoid collisions with people walking that fast. We can, however, anticipate these collisions as early as needed, with appropriate FoV and prediction horizon following the simple suggestions of [START_REF] Hohn | Probabilistic balance monitoring for bipedal robots[END_REF], and initiate an emergency stop.

B. Uncertainty in the motion of the crowd

Tables IV and V show, for passive safety and relaxed passive safety strategies respectively, the number of failures that take place when there is uncertainty in the perception of people. In each table, the first two columns are the values of uncertainty. The next two columns present the median velocities of both the robot and the person at the time of collision along the direction of the contact. Cases with no collision are indicated with a dash "-". The last column shows the proportion of simulations that ended with a failure.

The failure rate of the emergency stop strategy varies little with the uncertainty in position but increases with the uncertainty in velocity. Uncertainty causes this strategy to make unnecessary and undesirable stops that lead to collisions. In the deferrable emergency stop strategy, the failure rate seems unaffected by the uncertainty in position as well but decreases with the uncertainty in velocity.

It is important to note that, in our model of the crowd, the motion of people does not become more erratic with increased uncertainty. What increases is the size of the areas where people might be in the future. With only uncertainty in position, these areas have equal size throughout the horizon. People appear to be larger and there is less space to evade them. With only uncertainty in velocity, these areas increase in size along the preview horizon. People appear to be larger at the distance but reduce their size as the robot approaches them.

The velocity of the robot at the moment of collision is either zero or close to zero but negative, as expected. This suggests that the robot is trying to walk away from people before the collision happens.

The controller that implements Hierarchy 3 walks away from people as much as possible, even by following trajectories that lead to infeasible future states. This results in fewer collisions than any other strategy only because the robot is deciding to fall on the ground instead. On the other hand, with the controller that implements Hierarchy 2, the order of the priorities requires the motion of the robot to be dynamically and kinematically feasible during the whole prediction. The resulting number of collisions is higher but the total number of failures is lower.

The controller that implements Hierarchy 2 delays collisions as much as possible. The corresponding optimal control action is, in most cases, to make the robot walk away from people while in some other specific cases (e.g. when people Hierarchy relaxation of collision avoidance and feasibility are coming towards the robot from all directions at the same speed) it is to make it remain still. In any case, the difference of velocities at the moment of collision is guaranteed to be no bigger than it would be if the robot was at rest. Collisions are, therefore, less severe.

The typical problem solved at every iteration by the relaxed passive safety controllers on the scenario shown in Table III contains: 530 constraints distributed along 20 priority levels and 40 decision variables. It is solved, on average, in 10[ms] on a computer with a 3[GHz] Intel Core i7 processor.

VI. CONCLUSIONS

We designed controllers for the safe navigation of biped robots in crowds merging viability concepts from balance and collision avoidance. We developed two passive safety controllers and a new strategy that addresses the limitations of passive safety. All schemes were evaluated in simulations of walking crowds where we: 1) made recommendations on the minimal perception requirements of the robot to navigate safely; 2) demonstrated that failures could be previewed as early as possible and with no false alarms in perfectly known environments; 3) attested that the relaxed passive safety strategy produces less collisions than passive safety ones and, when they happen, they are less severe. Some aspects of this work can lead to further research: considering occlusions of people inside the FoV; reasoning about what happens after a collision takes place and how damages to people, the robot and the environment can be minimized.

Fig. 1 :

 1 Fig.1: Two different predictions of the motion of a person: without uncertainty (left) and with uncertainty in velocity (right). The gray areas are the predicted future positions of the person. When there is uncertainty in velocity, the set of possible positions the person could be in the future increase with time, following[START_REF]Robots and robotic devices -Safety requirements for personal care robots, ser[END_REF].

Fig. 2 :

 2 Fig.2: Collision avoidance. The vectors ûk define lines that the robot cannot cross. These lines create a convex hull (the light gray area) where the robot is allowed to move.

Hierarchy 2 : 1 : 2 : 1 :N- 1 :

 21211 Relaxed passive safety 1 Feasibility: F i , ∀i ∈ I Collision avoidance: C 1 . . . N: Collision avoidance: C N -1 N+1: Collision avoidance: C N , Stop: S N N+2: Objectives: O i , ∀i ∈ I Hierarchy 3: Relaxed passive safety 2 Feasibility: F 1 , Collision avoidance: C 1 . . . Feasibility: F N -1 , Collision avoidance: C N -1 N: Feasibility: F N , Collision avoidance: C N , Stop: S N N+1: Objectives: O i , ∀i ∈ I new measurements of the surrounding crowd, it is possible to postpone the stop of the robot, as long as possible.

Fig. 3 :

 3 Fig. 3: Walking against a crowd.

Fig. 4 :

 4 Fig. 4: Failure rate and median anticipation time for different speeds of the crowd and different durations of the horizon.

TABLE I :

 I Parameters of biped robot

	Parameter	Symbol	Value	Unit
	Height of CoM (4)	h	0.80	m
	Feet dimensions (5)	Z	(0.24, 0.14)	(m, m)
	Leg stride (6)	L	0.30	m
	Feet separation (7)	D	0.20	m
	Safety distance (9)	d 0	1	m
	Reference speed (O)	ċx ref ċy ref	0.5 0	ms -1 ms -1
	Step duration	-	0.8	s
	Sampling time	∆T	0.1	s
	TABLE II: Parameters of simulations with no uncertainty
	Parameter	Symbol	Value	Unit
	Horizon duration	H	{1.0, 1.8, 2.6}	s
	Radius of FoV	R	{0.5, 1, 2, 3, 4}	m
	Size of crowd	M	{8, 16, 32}	-
	Speed of the crowd	ṁx k ṁy k	{0.25, 0.5, 1, 1.5, 2}	ms -1

TABLE III :

 III Parameters of simulations with uncertainty

	Parameter	Symbol	Value	Unit
	Horizon duration	H	1.8	s
	Radius of FoV	R	4	m
	Size of crowd	M	16	-
	Speed of the crowd Uncertainty	ṁx k ṁy k mk mk	0.5 [-0.2, 0.2] {0, 0.15, 0.30} {0, 0.05, 0.10}	ms -1 ms -1 m ms -1

TABLE IV :

 IV Collisions of passive safety strategies

	Uncertainty	Collision vel[ms -1 ]	Failure	Uncertainty	Collision vel[ms -1 ]	Failure
	Pos[m]	Vel[ms -1 ] Robot	Person	rate[%]	Pos[m]	Vel[ms -1 ] Robot	Person	rate[%]
	0	0	-0.04	0.48	27	0	0	-0.05	0.36	32
	0	0.05	0.00	0.52	34	0	0.05	0.00	0.55	15
	0	0.10	0.00	0.41	36	0	0.10	0.00	0.55	9
	0.15	0	-0.01	0.44	28	0.15	0	-0.01	0.42	31
	0.15	0.05	0.00	0.40	38	0.15	0.05	0.00	0.52	15
	0.15	0.10	0.00	0.41	38	0.15	0.10	0.00	0.55	9
	0.30	0	0.00	0.46	34	0.30	0	0.00	0.45	35
	0.30	0.05	0.00	0.50	40	0.30	0.05	0.00	0.55	15
	0.30	0.10	0.00	0.41	39	0.30	0.10	0.00	0.53	9
		(a) Hierarchy 1: emergency stop		(b) Hierarchy 1: deferrable emergency stop

TABLE V :

 V Collisions of relaxed passive safety strategies

	Uncertainty	Collision vel[ms -1 ]	Failure	Uncertainty	Collision vel[ms -1 ]	Failure
	Pos[m]	Vel[ms -1 ] Robot	Person	rate[%]	Pos[m]	Vel[ms -1 ] Robot	Person	rate[%]
	0	0	-0.33	0.39	19	0	0	-0.60	0.39	21
	0	0.05	-0.71	0.60	4	0	0.05	-	-	11
	0	0.10	-0.53	0.59	2	0	0.10	-	-	13
	0.15	0	-0.19	0.49	15	0.15	0	-	-	22
	0.15	0.05	-0.74	0.58	5	0.15	0.05	-	-	10
	0.15	0.10	-0.64	0.58	2	0.15	0.10	-	-	13
	0.30	0	-0.14	0.56	15	0.30	0	-	-	27
	0.30	0.05	-0.48	0.58	5	0.30	0.05		-	12
	0.30	0.10	-0.28	0.57	2	0.30	0.10	-	-	13
	(a) Hierarchy 2: relaxation of collision avoidance

ACKNOWLEGMENTS

This work has been funded by the PSPC Romeo 2 project and EU H2020 Comanoid Research and Innovation Action (RIA).


Matei Is

Florent De

Automating the pipeline of arithmetic datapaths

This article presents the new framework for semiautomatic circuit pipelining that will be used in future releases of the FloPoCo generator. From a single description of an operator or datapath, optimized implementations are obtained automatically for a wide range of FPGA targets and a wide range of frequency/latency trade-offs. Compared to previous versions of FloPoCo, the level of abstraction has been raised, enabling easier development, shorter generator code, and better pipeline optimization. The proposed approach is also more flexible than fully automatic pipelining approaches based on retiming: In the proposed technique, the incremental construction of the pipeline along with the circuit graph enables architectural design decisions that depend on the pipeline.

I. INTRODUCTION

The FloPoCo project is an open-source generator of arithmetic cores for FPGAs. Its main purpose is to study the opportunities of tailoring arithmetic components to their application context. This includes parameterizing the operators in size, and offering them in a range of cost/performance trade-offs (Figure 1). It also requires flexibility in the latency/frequency trade-off, which can be achieved by pipelining. FloPoCo pioneered frequency-directed pipelining: the parameter controlling the pipeline depth is a user-provided target frequency. This enables the construction of complex pipelined operators out of smaller ones, all designed to work at the same frequency. FloPoCo is also vendor-neutral, supporting a range of FPGAs from Altera and Xilinx. Although its main point is to offer original operators, it must also achieve performance on par with vendor-provided tools.

The contribution of this article is a framework that addresses these needs. It enables the construction of high-quality pipelines for a wide range of frequencies on a wide range of targets. For this, it requires very limited design overhead on top of the description of the combinatorial operator.

This work is motivated by a review of existing pipelining techniques in Section II. Section III details the proposed pipelining construction framework. Section IV presents some The case study we use throughout the paper is the floatingpoint adder. It is a well-understood benchmark, and it requires several sub-components (shifters, leading zero counters, several integer adders of various sizes) and a long pipeline with the need to synchronize many signals.

Space prevents providing all the details, but the interested reader is encouraged to obtain the source code from gforge.inria.fr. This framework was developed in the newPipelineFramework branch of the git repository, and will be used in FloPoCo 5.0 and following.

II. BACKGROUND

Automatic circuit pipelining was formalized by Leiserson and Saxe [START_REF] Leiserson | Retiming synchronous circuitry[END_REF], who introduced the notion of retiming: moving a register from the output of a gate to its inputs (or conversely) doesn't change the function computed by the circuit, but may change its performance. This technique is complex to apply in practice (because of initialization values, multiple clocks, etc), and it took many years before commercial tools like Synplify Pro offered it. Its use in mainstream FPGA design tools is currently still limited to pushing registers inside DSP blocks or memory.

It is possible to pipeline a design purely by retiming, but if the goal is to achieve a desired frequency, one first has to determine how many pipeline levels must be inserted. One option is to obtain (by synthesis) the critical path delay of the combinatorial operator. Then, a lower bound on the the needed number of pipeline levels is obtained by dividing the critical path delay by the target period.

The FloPoCo framework [START_REF] De Dinechin | Designing custom arithmetic data paths with FloPoCo[END_REF] automated this task. The typical design flow was to first write a generator of combinatorial operators, then enrich it with constructs that 1/ estimate the global critical path of a circuit, 2/ automatically insert pipeline registers accordingly, and 3/ semi-automatically synchronize the resulting signals [START_REF] De Dinechin | Designing custom arithmetic data paths with FloPoCo[END_REF]. However, in this approach, synchronization as well as critical path construction were assisted but not automated. This required the designer to explicitely manage a global view of the whole datapath.

Altera's solution, briefly described in [START_REF] Chung | Tools and techniques for efficient high-level system design on FPGAs[END_REF], improved on this. Their method consists of first building a graph representing the circuit, annotated with local delay information. Then, a scheduler aggregates this local information to determine critical paths and automatically inserts pipeline registers in the graph in a greedy fashion, while keeping the graph synchronized. The authors identify three main issues. The first is the management of functional registers such as those delaying signals in a digital filter. These are part of the function of the filter and should be preserved, although they can be retimed. In [START_REF] Chung | Tools and techniques for efficient high-level system design on FPGAs[END_REF], they are ignored during the construction of the schedule, and only taken into account when generating the VHDL code describing the circuit. The second challenge is to manage loops in the circuit graph. Their approach is to break the loops during scheduling and reconnect them once the process is over. The third challenge is sub-components: they are pipelined separately.

Xilinx developed comparable tools at the same time, but contrary to Altera, they work with placed and routed circuits [START_REF] Gaide | Methods of pipelining a data path in an integrated circuit[END_REF]. This offers the best precision, but has a high computational cost. More recently, Xilinx introduced a new tool (report pipeline analysis) for automatically determining the pipelining potential of a circuit [START_REF] Ganusov | Automated extra pipeline analysis of applications mapped to Xilinx UltraScale+ FPGAs[END_REF]. It determines the maximum achievable frequency for the circuit, taking into account that circuit loops will limit it. This analysis tool currently only modifies the circuit in experimental versions of the Vivado design suite.

Matlab and Simulink [START_REF] Venkataramani | System-level retiming and pipelining[END_REF] also have an approach based on retiming [START_REF] Leiserson | Retiming synchronous circuitry[END_REF]. Their major contribution consists in removing the need to ensure the equivalence of the circuit states to the initial state. This allows them to considerably speed up the algorithm, while imposing certain limitations on the initial design.

The approach introduced in the present article inherits from FloPoCo's [START_REF] De Dinechin | Designing custom arithmetic data paths with FloPoCo[END_REF] and Altera's [START_REF] Chung | Tools and techniques for efficient high-level system design on FPGAs[END_REF]. It works at the HDL level. It requires a designer to add local timing information to the combinatorial circuit. The aggregation of this local information for synchronization and timing construction is then fully automated. Functional registers are also supported.

An originality of the present work is to allow for incremental scheduling. This is more flexible than all the previous approaches which start with the full circuit graph. Computing the circuit pipelining during the construction of the architecture, and not afterwards, opens new opportunities: the construction of the circuit graph itself may be optimized by taking into account its partial pipeline. Examples of applications will be reviewed in Section IV. Of course, it is still possible to schedule (or re-schedule) a complete graph in order to benefit from optimal scheduling algorithms, e.g. based on integer linear programming.

Another originality of the present work, demonstrated in the sequel, is that it works for a range of FPGAs and a range of vendor tools.

Finally, the proposed approach doesn't forbid post-placeand-route retiming of the circuits it produces for optimal performance.

III. PIPELINE CONSTRUCTION IN FLOPOCO

An overview of the pipelining methodology is presented on Fig. 2 vhdl << declare("signX") << "<= newX("<<wE+wF<<");"; vhdl << declare("signY") << "<= newY("<<wE+wF<<");"; vhdl << declare(target->logicDelay(), "effSub") << "<= signX xor signY;" ; (...) vhdl << declare(target->logicDelay(2), "excR", 2) << " (...) when effSub ='1' (...)" (...) vhdl<< declare(target->adderDelay(wE+1),"eXmeY", wE) << " <= (X" << range(wE+wF-1,wF)<<") -(Y" <<range(wE+wF-1,wF)<<");"; in Section III-F). . Fig. 3 shows an example extracted from the architecture description of a parametric floating-point adder. The resulting pipelined VHDL code is presented in Fig. 4 (a signal name ending in '_dxx' represents a signal delayed by xx cycles).

Fig. 3 shows how certain design parameters (here exponent and mantissa sizes wE and wF) are held in C++ variables: this is far more convenient than using VHDL generics.

Signals are declared using the declare() method. Its main purpose is to add a signal to a signal dictionary. Its parameters are a signal name, and an optional signal bit width (e.g. signal excR). In addition, a first optional argument is a delay (in seconds) that estimates the delay contribution δ τ of the right-hand side expression.

As Fig. 3 For instance, target->adderDelay(wE+1) returns an estimation of the delay of an addition of wE+1 bits. These methods will be detailed and evaluated in Section V.

B. The signal graph

A lexical analyzer (middle of Fig. 2) extracts the data dependencies between VHDL signal names in the left-hand side and right-hand side of each statement of the vhdl stream. It builds a signal graph (S-Graph): its nodes are the signal names and its edges correspond to such data dependencies. Fig. 5, produced by the tool, shows the S-Graph for a singleprecision floating-point adder, with a zoom on the part that corresponds to Fig. 4. For example, in Fig. 4, effSub depends on signals signX and signY. Thus, the signal graph contains edges from the nodes signX and signY to the node effSub.

Moreover, each signal node is labelled with its contribution to the critical path (which is the delay passed to declare()). This is the number on the second line of each box on Fig. 5.

For the sake of readability, Fig. 5 is a compact representation of the graph, where the graphs of the sub-components are omitted. FloPoCo can also output the full S-Graph with all sub-components flattened.

The lexer also annotates the architecture's VHDL code to facilitate further passes over the architecture which will replace signals with their registered version (those whose name ends with _dxx on Fig. 4).

The next step on Fig. 2 is to compute the scheduling of the S-graph so that it corresponds to a circuit pipelined for the target frequency. This consists in assigning to each signal the timing at which it will be computed by the circuit. Let us now describe how this timing is determined.

C. Lexicographic time

In a combinatorial circuit, the timing of a signal s can be computed by accumulating the delays on the longest simple path from the earliest input to s (critical path delay). Fig. 5: S-Graph for a single-precision floating-point adder Inside a pipelined circuit, however, a signal s may be delayed by a number of cycles c. The timing of s is then expressed as a pair (c, τ), where

• c is an integer that counts the number of registers on the longest path from an input to s. • τ is a real number that represents the critical path delay (in seconds) from the last register or earliest input to s.

The colors on Fig. 5 indicate the cycle, and the complete lexicographic time of each signal is given by the third line of each signal box.

There is a lexicographic order on such timings:

(c 1 , τ 1 ) > (c 2 , τ 2 ) if c 1 > c 2 or if c 1 = c 2 and τ 1 > τ 2 .

D. Incremental scheduling

The scheduling of a circuit is a well-studied problem, which also bears resemblance to many other similar computer science problems. In our case it consists in assigning to each signal of the S-graph a lexicographic time that respects causality along the dependencies of the S-Graph. There are several possible objective functions, essentially minimizing the output timings (i.e. optimizing for performance), or minimizing the number of registers (i.e. optimizing for resources). Our implementation currently focusses on the former.

The difficulty is that we consider an incremental scheduling problem, where signals arrive over time. The total number of signals is not known in advance. A signal's critical path contribution becomes known upon its insertion in the S-graph. However, it may happen that some of of the dependency information concerning a signal is still unknown upon its arrival.

The approach chosen is a greedy, as-soon-as-possible solution. Signals are scheduled as soon as their predecessors are scheduled. They are then assigned the earliest possible timing. This scheduling process is outlined in Algorithm 1. end for 18: end procedure

Algorithm 1 S-Graph Scheduling

The populate_signals_to_schedule() function on line 2 selects from the signal dictionary the signals that have been affected by the latest vhdl stream operation. These are the signals that need to be scheduled, together with their direct and transitive successors. Note that a signal might be rescheduled. This is due to the incremental nature of the design process, and it ensures compatibility with VHDL's way of specifying concurrent instructions.

The calls to schedule_signal() on line 7 recursively trigger the scheduling process. It starts with the signals selected on line 2 and propagates to their dependences, as per the S-Graph, and depicted in lines 15-17. Lines 12-14 describe how a signal's timing is chosen: it is the smallest (lexicographically) pair (c, τ) which satisfies the following constraints:

(c, τ) ≥ δ τ + (c pred , τ pred ), ∀pred ∈ predecessors(signal).
This is where new pipeline levels are added: the addition in the previous equation is a lexicographic addition for a cycle of latency 1/ f , where f is the target frequency. Defining δ obj = 1/ fδ ff where δ ff is the delay of a register, the lexicographic time addition is:

(c, τ) + δ = c + τ + δ δ obj , τ + δ δ obj - τ + δ δ obj
When rescheduling a signal, in addition to the original constraint, a similar constraint must be met with respect to the successors: (c, τ) ≤ (c succ , τ succ )δ τ (succ), ∀succ ∈ successors(signal) .

E. Sub-components

An operator constructor can also launch a chain of constructor calls (top left part of Fig. 2. Each subcomponent constructor builds its S-Graph, which is then integrated into the S-Graph of the parent operator. The result is that a hierarchy of components can also be managed as a fully flattened S-Graph.

Subcomponents are, by default, inlined, each instance of the same component being scheduled independently (line 4). This provides the best performance, as the schedule of an instance may depend on the schedule of its inputs. However, for components reused many times, there is also the option to schedule only once each component and reuse this schedule for all instances. This prevents a size explosion of the S-DAG (and the generated VHDL) in such cases, but imposes that pipeline stages inside such shared-schedule instances will be identical. This option is essentially used for elementary (atomic) components such as compressors which are too small to be pipelined anyway.

F. Functional registers

Using the delay() method, designers may also explicitely insert functional registers. These will be inserted during the final VHDL generation in addition to the pipeline registers: functional registers don't affect scheduling.

IV. CIRCUIT DESIGN WITH TIMING INFORMATION

The ability to use timing information during circuit construction is the major motivation for the incremental scheduling approach presented here. The bit heap framework, introduced in [START_REF] Brunie | Arithmetic core generation using bit heaps[END_REF] and improved in [START_REF] Kumm | Pipelined compressor tree optimization using integer linear programming[END_REF], illustrates this need, because bits will arrive in the bit heap at various lexicographic times. For instance, when a multiplier is built out of DSP blocks and logic, it must add bits coming from DSP blocks after two or three cycles, and bits that arrive from LUTbased multipliers after one LUT latency only. Another example would be a FIR where each constant product comes from a highly constant-dependent shift-and-add architecture, with a correspondingly constant-dependent timing. In all such cases, an optimal compressor tree should group together bits of similar lexicographic time: the construction of the circuit graph itself depends of the schedule of a part of the graph.

Circuits with loops, such as IIR filters, are another class of circuits that benefit from incremental scheduling. The chosen solution is to assume that a designer knows that there is a loop in the circuit: loops must be tagged as such by the designer in the constructor code. This results in immediate feedback on the maximum achievable frequency, which can be further used to constraint the target frequency of the rest of the circuit.

V. TARGET MODELS

The Target virtual class tries to encapsulate the performance capabilities of the various target FPGAs in a way that is as generic as possible. It defines virtual methods that can be used in operator constructor code. These methods are implemented in the actual instances of the Target class.

In the new framework, the design choices were motivated by genericity to three different targets, each with a different design suite: Virtex-6 with ISE, Kintex-7 with Vivado, StratixV with Quartus II. We also tried to pave the way for ASIC target classes.

Here are some methods used in this work, by order of importance. Each of them returns a delay in seconds.

• ffDelay() returns the delay of a flip-flop. So far, this method just returns a constant. • logicDelay(n) returns an estimation of the delay of an arbitrary logic function of n arguments. Its implementation is architecture dependent, reflecting for instance the hierachy of muxes that can be used inside a Xilinx slice before having to go to the general routing. This method supersedes the less generic lutDelay() of previous versions.

• adderDelay(n) returns the delay of an addition of size n. Again the implementation is very different for our three targets: Fast carry logic has a granularity of 1 on Virtex-6, 4 on Kintex-7, and 10 on StratixV.

• wideOrDelay(n), eqComparatorDelay(n), eqConstComparatorDelay(n) attempt to capture the use of fast-carry logic to implement wide OR and wide AND operations. For each of these methods, the returned delay now also includes an estimation of the local routing delay. In previous versions, such delays had to be added in constructor code, which turned out to be very repetitive.

Capturing the delay of embedded memories and DSP blocks is more complex, since these blocks have internal registers, various chaining possibilities, dual-porting, etc. These features are best used through FloPoCo operators such as Table and IntMult, which take care of this complexity. Inside these operators, we are not ashamed to have target-specific ad-hoc implementations.

Table I evaluates the accuracy of these models. The first line of each target shows that prediction of small logic units (here one single addition) is very accurate. A 63-bit barrel shifter consists of 6 logic levels, but the constructor code of the corresponding operator attempts to pack two or three of them in a LUT, depending on the number of LUT inputs reported by the lutInputs() method of Target. As the second line of each target shows, this prediction remains relatively accurate. Finally, for a complete floating-point adder, the tools find more opportunities to fuse logic in large LUTs, and our predictions become very pessimistic. However, as Table II shows, in a pipelined operator, there are few logic delays in each level, and the models, again, work well.

Let us end this section with something that doesn't work well: taking into account large fanouts. We do have a fanoutDelay(n) method, but its current implementation is very arbitrary. The problem is that there are many, many ways, in modern FPGAs, to route large fanout signals, and the tools do exploit this freedom when routing for a prescribed clock constraint. As a consequence, although the tools do report fanout information and the associated delays in the detailed critical path timing reports, it seems impossible to turn this information into an accurate model: firstly, the fanout mentioned there have very little relationship with the logical fanout. For instance, in the 63-bit shifter, we expect several large fanout signals which are increasing powers of two, but the largest fanout reported is 5. Secondly, there is no strict relationship between the fanout and the reported delay, as Figure 6 illustrates: we have there fo=8, delay=2.17ns as well as fo=143, delay=1.086ns. The current choice is an adhoc linearization of this figure, but it will obviously be very inaccurate. The good news is that the tools seem to be able to compensate if we underestimate the fanout delay, so this is the sensible thing to do.

VI. RESULTS

The main positive result of this work is the simplification of the design of complex operators. FPAddSinglePath.cpp, which describes our floating-point adder case study, was reduced from 557 down to 472 lines of code. Many of these lines were dedicated to explicit synchronization management, which is no longer required. Having to worry only about local timing information makes life much simpler.

Generation time is still very fast, with all operators in this article generated in a fraction of a second. Working at the level of VHDL signals, we have much fewer signals to manage than working at the level of the gate or the bit. Of course, this aggregated timing information is less accurate. This probably explains the slightly better results of Xilinx IP in Table I.

Still, Table III shows that in most cases, the new pipeline framework improves the generated pipelines by reducing the latency, increasing the frequency and reducing resource consumption.

This data was obtained using the following command line:

flopoco target=StratixV frequency=400 \ FPAdd we=8 wF=23 Wrapper

The Wrapper operator simply adds registers on the inputs and outputs of the previous FPAdd. The synthesis results themselves were obtained for StratixV (5SGXEA3K1F35C1) using Quartus 16.0 and the tools/quartus_runsyn.py utility of FloPoCo. They should be reproducible. Since the only difference between both versions is in the pipeline, the combinatorial operator is identical between them. Its clock constraint was set to 400 MHz.

VII. CONCLUSION

By raising the abstraction level offered to the designer, this work allows her to write, with very little effort, generic code that produces complex pipelines of high quality for a range of targets and a range of frequencies. The quality of the generated IP is on par with that of vendor tools, but of course the purpose Working with the new generation of back-end tools from Altera and Xilinx has proven very challenging. However, as these tools require a clock constraint to implement a design, there is a clear convergence here with the frequency-directed optimization philosophy of FloPoCo.

Current effort focuses on refining the target models further, completing the upgrade of the bit heap into this framework, and porting all the operators that depend on it. This includes large multipliers and squarers, constant multipliers, various elementary functions and function generators, and filters.

With a solid management of pipelines with loops, we also plan to take FloPoCo beyond its initial domain into signal processing.

Fig. 1 :

 1 Fig. 1: Interface to FloPoCo operators

Fig. 2 :

 2 Fig. 2: Constructor flow overview

Fig. 3 :

 3 Fig. 3: Example C++ constructor code

Fig. 4 :

 4 Fig.4: VHDL code generated by the code of Fig.3

Fig. 6 :

 6 Fig. 6: Plots of net delay versus fanout in the floating-point adder on Kintex-7 (placed and routed design).

TABLE I :

 I Accuracy of our target models

	target		performance	resources
	StartixV	FloPoCo	7+2 cycles @ 421 MHz 492R + 231L
	/ Quartus	ALTFP 16.0	7+2 cycles @ 333 MHz 477R + 358L
	Virtex6	FloPoCo	8+2 cycles @ 408 MHz 470R + 461L
	/ ISE	FloatingPoint6.1 8+2 cycles @ 427 MHz 500R + 416L
	Kintex7	FloPoCo	8+2 cycles @ 401 MHz 463R + 339L
	/ Vivado	FloatingPoint7.1 8+2 cycles @ 476 MHz 499R + 334L
		FloatingPoint7.1 6+2 cycles @ 417 MHz 384R + 344L

TABLE II :

 II Pipelining a floating-point adder with registers on the I/O, for three different targets using three different vendor tools. For each target, the FloPoCo operator is compared to the equivalent vendor IP.

TABLE III :

 III Comparison between old and new FloPoCo on FPAdd (single and double precision) with registers on the I/Os. of FloPoCo is to keep researching arithmetic operators not provided by vendor tools.

ACKNOWLEDGEMENTS

This work was supported by the French National Research Agency through the INS program MetaLibm.


Alexander Sherikov
email: alexander.sherikov@inria.fr

Dimitar Dimitrov
email: dimitar.dimitrov@inria.fr

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

Balancing a humanoid robot with a prioritized contact force distribution

Humanoid robots propel themselves and perform tasks by interacting with their environment through contact forces. Typically, nonuniqueness of these forces is dealt with by distributing them evenly between the contacts. In the present paper, we introduce strict prioritization in contact force distribution, to reflect situations when an application of certain contact forces should be avoided as much as possible, for example, due to a fragility of the support. We illustrate this by designing a whole body motion controller for a setting with multiple noncoplanar contacts, where application of an optional contact force is allowed only if it is necessary to maintain balance and execute a task. Balance preservation is addressed by imposing a capturability constraint based on anticipation with a linear model adapted to multiple noncoplanar contacts. The controller is evaluated in simulations.

I. INTRODUCTION

Humanoid robots belong to the class of floating base systems, and, consequently, the contacts with the environment play a crucial role in their control. The interaction with the environment can be studied by focusing on different aspects such as force distribution between contacts, balance preservation, contact planning for realization of locomotion and manipulation tasks. In the present paper we primarily address the first two aspects in a setting with multiple noncoplanar contacts.

Let us consider the problem of force distribution. In most settings with multiple contacts there exists an infinite number of force distributions that achieve the same base motion. The typical approach to resolve this ambiguity is to make contacts as robust as possible, by keeping each contact force far from the bounds of the respective friction cone, and distribute the forces evenly between all the contacts [START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF], [START_REF] Ott | Posture and balance control for biped robots based on contact force optimization[END_REF], [START_REF] Herzog | Balancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics[END_REF], [START_REF] Hyon | Full-body compliant human humanoid interaction: Balancing in the presence of unknown external forces[END_REF].

There are situations however, such as when a contact area is fragile, when it is preferable to avoid using it unless strictly necessary for balance. In this case, distributing forces evenly between all possible contacts should be avoided. We propose therefore to introduce a prioritized distribution of the contact forces, with the help of hierarchical optimization [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], [START_REF] Kanoun | Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task[END_REF]. We demonstrate our idea in the following setting: a humanoid robot has to reach a target with a hand while standing, it can optionally exploit a contact of the other hand with an additional support to maintain balance. The control goal can be expressed with the informal hierarchy Hierarchy [START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF] 1: maintain balance 2: reach the target 3: minimize optional contact force which allows the application of an optional contact force, but only if it is necessary to accomplish the reaching task and maintain balance.

The highest priority in Hierarchy 1 is to preserve balance, i.e., avoid falling. A standard approach for achieving this is to maintain an adequate configuration of the body or position of the Center of Mass (CoM) [START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF], [START_REF] Ott | Posture and balance control for biped robots based on contact force optimization[END_REF], [START_REF] Hyon | Full-body compliant human humanoid interaction: Balancing in the presence of unknown external forces[END_REF], [START_REF] Collette | Robust balance optimization control of humanoid robots with multiple non coplanar grasps and frictional contacts[END_REF], [START_REF] Henze | Posture and balance control for humanoid robots in multi-contact scenarios based on model predictive control[END_REF], but this restricts the motions of the robot. A less restrictive alternative is to constrain the CoM to project vertically inside the support area [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], [START_REF] Bretl | Testing static equilibrium for legged robots[END_REF]. However, this is valid only for quasi-static motions. In the case of dynamic motions, balance typically requires to use some form of anticipation [START_REF] Sherikov | Whole body motion controller with long-term balance constraints[END_REF], [START_REF] Nagasaka | Whole-body control of a humanoid robot based on generalized inverse dynamics and multicontact stabilizer that can take account of contact constraints[END_REF], [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF], [START_REF] Mansour | A new optimization based approach for push recovery in case of multiple noncoplanar contacts[END_REF], [START_REF] Dai | Whole-body motion planning with centroidal dynamics and full kinematics[END_REF], [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF], [START_REF] Sentis | Compliant control of multicontact and center-of-mass behaviors in humanoid robots[END_REF].

Though anticipation using a whole body model is possible [START_REF] Dai | Whole-body motion planning with centroidal dynamics and full kinematics[END_REF], [START_REF] Tassa | Synthesis and stabilization of complex behaviors through online trajectory optimization[END_REF], [START_REF] Romano | Prioritized Optimal Control: a Hierarchical Differential Dynamic Programming approach[END_REF], it necessitates the use of nonlinear optimization, which can be computationally expensive and is still in the process of being extended to hierarchies [START_REF] Romano | Prioritized Optimal Control: a Hierarchical Differential Dynamic Programming approach[END_REF]. For these reasons we resort to anticipation with a simplified linear model. Note that such models are widely adopted precisely because they are usually sufficient for ensuring balance preservation [START_REF] Sherikov | Whole body motion controller with long-term balance constraints[END_REF], [START_REF] Nagasaka | Whole-body control of a humanoid robot based on generalized inverse dynamics and multicontact stabilizer that can take account of contact constraints[END_REF], [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[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]. In the case of multiple noncoplanar contacts, the simplified models typically involve the contact forces, but neglect the robot's structure and tasks associated with it, such as the reaching task in Hierarchy 1 [START_REF] Nagasaka | Whole-body control of a humanoid robot based on generalized inverse dynamics and multicontact stabilizer that can take account of contact constraints[END_REF], [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF], [START_REF] Mansour | A new optimization based approach for push recovery in case of multiple noncoplanar contacts[END_REF]. As a result, the Hierarchy 1 needs to account for two models at the same time, a whole body model for the reaching task, and a simplified model for balance preservation. A similar approach has already been used in [START_REF] Sherikov | Whole body motion controller with long-term balance constraints[END_REF] in the case of walking on a flat ground. It can be seen as a whole body Model Predictive Control (MPC), where the whole body model is replaced by a simplified linear model everywhere except the current time instance.

In the present work, we develop an online whole body motion controller with contact prioritization in accordance with Hierarchy 1. The controller is similar to the one presented in [START_REF] Sherikov | Whole body motion controller with long-term balance constraints[END_REF], but employs a linear model designed to deal with multiple noncoplanar contacts [START_REF] Nagasaka | Whole-body control of a humanoid robot based on generalized inverse dynamics and multicontact stabilizer that can take account of contact constraints[END_REF], [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF]. We construct a capturability constraint to be imposed at the end of the preview. This is important for balance preservation, as recognized in [START_REF] Sherikov | Whole body motion controller with long-term balance constraints[END_REF], [START_REF] Henze | Posture and balance control for humanoid robots in multi-contact scenarios based on model predictive control[END_REF], [START_REF] Mansour | A new optimization based approach for push recovery in case of multiple noncoplanar contacts[END_REF] and demonstrated here in simulation. The controller assumes that the contact positions are given by an offline planner such as [START_REF] Dai | Whole-body motion planning with centroidal dynamics and full kinematics[END_REF], [START_REF] Bouyarmane | Potential field guide for humanoid multicontacts acyclic motion planning[END_REF]. Since the exact time, when the optional contact is used, is not known in advance, we also assume that the support hand is already at the contact position from the beginning.

The paper is organized as follows. The components of the controller are described in Section II, which focuses on inverse dynamics control, and Section III, where the balance preservation is discussed. The following Section IV presents formalized Hierarchy 1 using the designed controller. Its performance is verified in simulations described in Section V. The final Section VI concludes the paper.

II. STANDARD WHOLE BODY MOTION CONTROL

This section briefly describes constraints and objectives used in the standard inverse dynamics approach to control of humanoid robots [START_REF] Fujimoto | Proposal of biped walking control based on robust hybrid position/force control[END_REF].

A. Lagrangian dynamics

The dynamics of a humanoid robot having M contacts with the environment can be described as

  H 1 H 2 H 3   H   q′ ẍ θ   q -   0 g 0   +   h 1 h 2 h 3   h =   τ 0 0   +   J T c,1 J T c,2 J c,3   J T c f (1)
where H is the generalized inertia matrix; q is the vector of joint angles q ′ , position x of the base of the robot, and Euler angles θ representing orientation of the base; g is the vector of gravitational acceleration; h is the vector of Coriolis and centrifugal effects; τ is the vector of joint torques; f is the vector of contact forces f i and moments µ i (i = 1, . . . , M ); and J c is the corresponding Jacobian matrix.

B. Joint limits

Joint accelerations and torques cannot take arbitrary values due to hardware limitations and therefore must be constrained. We bound the joint torques and accelerations using

τ ≤ τ ≤ τ , q′ ≤ q′ ≤ q′ , (2)
where the lower and upper bounds q′ and q′ are constructed to reflect joint position, velocity, and acceleration limits all together [START_REF] Rubrecht | Motion safety and constraints compatibility for multibody robots[END_REF].

C. Contact constraints

Control inputs consistent with the dynamics (1) and respecting hardware limitations may still cause a slip or a loss of contact with the environment, which can be prevented using additional constraints [START_REF] Fujimoto | Proposal of biped walking control based on robust hybrid position/force control[END_REF]. In these constraints, we denote the contact forces and moments as fi and μi when they are expressed in the frames fixed to the contact points p i , with the z axes normal to the contact surfaces.

• The contact forces are subject to friction cone constraints expressed using a linear approximation of the Coulomb friction cone

G i fi ≤ 0, (3)
where the rows of G i are the normal vectors to the edges of the pyramid approximating the friction cone.

• The moment about the z axis is limited due to torsional friction with

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

• The moments about x and y axes cannot be arbitrary as well. The ratio between them and the normal force f z i defines the Center of Pressure (CoP), which must stay within the contact support area. Using a linear approximation of the support area boundary, this constraint can be expressed as

D i fi μi ≤ 0. ( 5
)
where D i contains coefficients of the bounding lines [START_REF] Fujimoto | Proposal of biped walking control based on robust hybrid position/force control[END_REF], [START_REF] Nagasaka | Whole-body control of a humanoid robot based on generalized inverse dynamics and multicontact stabilizer that can take account of contact constraints[END_REF]. • In order to ensure that the contacts do not slip or detach, the accelerations of contact points are set to zero:

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

D. Control objectives

We use two simple control objectives throughout this paper. The first one drives a hand to a desired position with the help of a PD controller

J rh q = Λ p (x target -x) -Λ d ẋ -Jrh q, (7)
where Λ d and Λ p are positive scalar gains, x and ẋ are the the current position and velocity of the end-effector, and x target is its target position. The second one maintains a reference configuration q ref of the robot with a joint PD-

controller q = K p (q ref -q) -K d q, (8)
where K d and K p are positive scalar gains.

III. BALANCE PRESERVATION

Similarly to [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] we analyze the problem of balance preservation in terms of viability theory [START_REF] Aubin | Viability Theory: New Directions[END_REF]. A state is defined as viable, if there exists at least one evolution starting from this state and not resulting in a fall. In practice, however, most states of interest, when walking, are 2or 3step capturable, what means that the robot is able to stop in 2 or 3 steps [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] Zaytsev | Two steps is enough: no need to plan far ahead for walking balance[END_REF]. Based on this observation, we already proposed to preview the future motion of the robot, and impose that it must be able to stop some time (corresponding to 2 or 3 steps) in the future [START_REF] Sherikov | Whole body motion controller with long-term balance constraints[END_REF], i.e., be able to reach a statically balanced state. Note that, similarly to the terminal constraints, which are important components of MPC schemes [START_REF] Mayne | Constrained model predictive control: Stability and optimality[END_REF], we impose that the robot stops at a time in the future which is continuously postponed. As a result, it is not imposed that the robot actually stops, only that it maintains the capacity to stop, i.e., capturability.

Motion preview with the whole body dynamical model is a computationally expensive problem, and a simplified model can be used instead. In [START_REF] Sherikov | Whole body motion controller with long-term balance constraints[END_REF] we used a linear model tailored for walking on a flat ground. This same model, however, cannot be employed in a setting with multiple noncoplanar contacts. That is why here we rely on a different model derived from the relationship between the momenta of the robot and the contact forces [START_REF] Nagasaka | Whole-body control of a humanoid robot based on generalized inverse dynamics and multicontact stabilizer that can take account of contact constraints[END_REF], [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF], [START_REF] Dai | Whole-body motion planning with centroidal dynamics and full kinematics[END_REF].

A. Preview of momenta of the robot

The lower parts of the dynamics (1) are the Newton and Euler equations of the whole system [START_REF] Wieber | Holonomy and nonholonomy in the dynamics of articulated motion[END_REF], involving the linear P r and angular L r momenta of the whole robot with respect to the chosen base reference point r

Ṗ r T T E L r = H 2 H 3 q + h 2 h 3 = J T c,2 J T c,3 f + H 2 H 3   0 g 0   , (9)
where T E is a matrix transforming derivatives of the Euler angles used to represent orientation of the base to the angular velocities. It is more convenient to work with momenta P and L expressed in a frame fixed to the CoM [START_REF] Orin | Centroidal dynamics of a humanoid robot[END_REF]. By performing the necessary transformations of ( 9) we obtain

Ṗ L = M i=1 I 0 (p i -c) × I f i µ i + mg 0 , ( 10
)
where I is an identity matrix, (•) × denotes the skewsymmetric matrix corresponding to vector cross product, c is the position of the CoM, m is the mass of the robot.

As before, M is the number of contacts and p i are their positions. Equation ( 10) can be readily used for motion planning [START_REF] Dai | Whole-body motion planning with centroidal dynamics and full kinematics[END_REF], but its nonlinearity entails relatively high computational cost. Instead, we employ a linear model with the following assumptions [START_REF] Nagasaka | Whole-body control of a humanoid robot based on generalized inverse dynamics and multicontact stabilizer that can take account of contact constraints[END_REF], [START_REF] Audren | Model preview control in multi-contact motionapplication to a humanoid robot[END_REF]:

• the CoM is constrained to a horizontal plane M i=1 f z i = mg, (11)
provided that the gravitational acceleration is aligned with the z axis, i.e., g = (0, 0, -g); • the equation for the rate of angular momentum about the z axis can be safely neglected. The evolution of the momenta over N sampling intervals can be previewed using a discrete-time form of this model:

  Pxy k+1 P xy k+1 L xy k+1   = A   Pxy k P xy k L xy k   + M k i=1 B i,k f i,k µ i,k , (12)
where P = mc is the product of the CoM position by the mass of the robot, superscripts (•) x , (•) y , and (•) z denote coordinates, and k = 0, . . . , N -1. Since the number of contacts and their configurations do not change in our setting, the system is simplified due to M k = M, B i,k = B i . The preview starts from the current state ( Pxy 0 , P xy 0 , L xy 0 ), which can be easily determined from P r and L r . Control inputs applied on the first sampling interval are the contact forces and moments participating in the whole body dynamics (1), i.e., f = (f 1,0 , µ 1,0 , . . . , f M,0 , µ M,0 ). The control inputs remain constant during each sampling interval in the preview, and they must comply with constraints (3), ( 4), [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], as well as [START_REF] Nagasaka | Whole-body control of a humanoid robot based on generalized inverse dynamics and multicontact stabilizer that can take account of contact constraints[END_REF]. In the end of preview horizon the final state ( Pxy N , P xy N , L xy N ) and controls (f xy i,N -1 , µ xy i,N -1 ) are subject to static balance constraints, which are presented in the next subsection.

B. Static balance constraints

The key idea of our balance preservation approach is to ensure that commands computed at each control sampling time allow the robot to stop, i.e., reach a statically balanced state. For this purpose we impose the following static balance constraints on the final previewed state and controls of the linear model presented in the previous subsection

• the CoM velocity is zero in the x-y plane, i.e., the linear momentum is zero

P xy N = 0; (13)
• the CoM acceleration is zero in the x-y plane, i.e., the total force applied to the CoM is zero:

M f xy i,N -1 = 0; (14)
• the angular momentum is zero about x and y axes:

L xy N = 0; (15)
• the rate of change of the angular momentum about x and y axes is also zero:

Px N g - M i=1 f z i,N -1 p x i -µ y i,N -1 Py N g - M i=1 f z i,N -1 p y i + µ x i,N -1 = 0. ( 16
)

IV. A HIERARCHY WITH FORCE PRIORITIZATION

Given all the components described in Sections II and III, we can formalize Hierarchy 1 as follows:

Hierarchy (2) 1: Equality constraints • dynamics (1)
• fixed contacts [START_REF] Kanoun | Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task[END_REF] • zero vertical acceleration (11) 2: Inequality constraints 4), (5) 3: Static balance constraints ( 14), ( 13), ( 15), ( 16) 4: Reaching task (7) 5: Minimization of the optional contact force 6: Final objectives

• joint limits (2) • contact constraints (3), (
• Maintaining the reference configuration using [START_REF] Henze | Posture and balance control for humanoid robots in multi-contact scenarios based on model predictive control[END_REF], weighted with the inertia matrix H • Minimization of all contact wrenches Decision variables of this optimization problem are the joint torques τ , the generalized accelerations q, and contact wrenches (f i,k , µ i,k ) for each i-th contact and k-th sampling interval of the preview horizon. The joint torques are eliminated from computations as in [START_REF] Herzog | Balancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics[END_REF]. The exact definition of the objective to minimize on the 5-th level concerning the optional contact force is left for Section V.

V. SIMULATIONS

We evaluate Hierarchy 2 in simulations using the HRP-2 robot [START_REF] Kaneko | Humanoid robot hrp-2[END_REF], whose behavior can be examined in the accompanying video.

All tests presented in this section are performed in the same general setting: the robot is standing with its left hand positioned on an additional support, while the right hand either has to move to a given target position (Figure 1) or maintain its position in presence of disturbances (Figure 6). When the goal is to reach a target position, the target is initially set to be far from the robot and then moved closer as demonstrated in Figures 1 and2. In all cases the controller is expected to apply a force on the additional support only if necessary to preserve balance.

We have observed that particular choices of the preview horizon length and its discretization have little effect on the overall behavior of the robot across all our tests. In all the following tests the MPC parameters are set as follows: the number of sampling intervals in the preview N = 4, with a duration of one interval T = 100ms.

In order to successfully complete these tests, it is necessary to regularize Hierarchy 2 in the presence of conflicts between its levels. Otherwise, the joint accelerations start to flip between the bounds and the right hand task is poorly executed. We perform regularization of the levels 4 and 5, which may conflict with each other and the balance constraints, using the weighted objective from the last level 6.

A. Test 1

Here, the right hand has to reach a moving target position, as demonstrated in Figures 1 and2. Before this can be achieved, Hierarchy 2 has to be completed by defining its 5-th level. One possible way to do this is to perform a minimization of the current contact force, using

f opt,0 = 0. (17)
However, a controller based on such formulation does not produce the desired behavior since it does not use its knowledge of the future adequately.

First of all, the inequality constraints on the level 2 become infeasible during the simulation. The reason for this is that the previewed optional contact forces can take large values, which cannot be applied by the robot. We address this problem by introducing upper bounds on the normal components of the previewed optional contact forces

f ⊥ opt,k ≤ fmax , k = 0, . . . , N -1 (18)
on the 2-nd level of Hierarchy 2. This helps to reflect the hand joint limitations, which cannot be directly expressed in the preview. Though the proposed modification helps to avoid infeasibilities, it is still not sufficient to obtain the desired behavior: the increase of the contact force is very sharp (Figure 3) and the CoM (Figure 5) does not return close to the initial position after the hand target is moved back.

The sharp increase of the contact force occurs due to the fact that the objective [START_REF] Romano | Prioritized Optimal Control: a Hierarchical Differential Dynamic Programming approach[END_REF] favors large future contact forces f opt,k (k = 1, . . . , N -1) in order to minimize the current force f opt,0 . We can see in Figures 3 and5 that once the CoM leaves the support area of the feet, the robot starts falling forward until it is absolutely necessary to apply the current optional contact force f opt,0 to stop falling.

In the second part of the motion the objective [START_REF] Romano | Prioritized Optimal Control: a Hierarchical Differential Dynamic Programming approach[END_REF] prevents the controller from momentarily increasing the current optional contact force f opt,0 , what is necessary to move the CoM back towards the initial state.

B. Test 2

In order to address the problems indicated in Test 1, we propose to minimize the optional contact force over the whole preview horizon:

f opt,k = 0, k = 0, . . . , N -1. (19)
By keeping the previewed optional contact forces small, we remove the need for additional feasibility constraints [START_REF] Wieber | Viability and Predictive Control for Safe Locomotion[END_REF] in the considered setting. Furthermore, the objective (19) allows a larger current contact force if it helps to decrease the contact forces in the future. The obtained controller works completely in accordance with our expectations (see Figure 1). Initially, the optional contact force is zero and starts increasing at second 5 of the simulation until the target is reached (see Figures 3,2), after the change in position of the target the force gradually returns to zero. Note that contrary to the Test 1, the moment of the contact activation is very close to the moment when the CoM projection leaves the foot support area, as demonstrated in Figure 5. Thus, the optional contact force is applied before it is strictly necessary thanks to the anticipation done in the objective [START_REF] Koolen | Capturability-based analysis and control of legged locomotion, part 1: Theory and application to three simple gait models[END_REF].

C. Test 3

The controller used in Test 2 also produces a satisfying behavior in the presence of perturbations. We verify this in a setting where the right hand has to maintain a constant position under action of an external force f ext , as shown in Figure 6. Thus, the goal of the controller is to maintain balance while holding a heavy object, such as a bucket filled with some weight [START_REF] Stephens | Dynamic balance force control for compliant humanoid robots[END_REF]. The external force is assumed to be measured and its contribution is accounted for in the dynamical model [START_REF] Saab | Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[END_REF], but not in the preview. The evolution of the optional contact force and the external force are shown in Figure 4. The external force of 20N is initially applied at 0.25s, 2 seconds later it starts to grow during 1 second until it reaches 60N, and is set back to 0 at 6.25s. The optional contact force changes smoothly in response to the disturbance, and eventually returns to 0.

D. Test 4

Finally, Test 2 was repeated without the static balance constraints (level 3 of Hierarchy 2), to demonstrate their importance for balance preservation. During the simulation the robot falls towards the target as can be seen in Figures 3,5 eventually causing a major violation of the constraints.

VI. CONCLUSION

We designed a whole body motion controller with contact force prioritization and balance constraints for general multicontact scenarios. The controller was evaluated in simulations, which demonstrated that it is capable of applying an optional contact force only when necessary to execute tasks and maintain balance. Furthermore, we indicated that some minor design choices can lead to unsatisfactory behavior, and additional constraints on the previewed motion may be necessary to reflect the whole body limitations.

Fig. 1 :

 1 Fig. 1: Test 2: configurations of the robot during the simulation. Grey areas represent contact surfaces.

Fig. 2 :

 2 Fig. 2: Right hand position along x (blue), y (green), z (red) axes. The desired positions are shown in dotted lines. Solid curves indicate trajectories executed in test 2.

Fig. 3 :

 3 Fig.3: Norm of the optional contact force f opt,0 in test 1 (dashed blue), test 2 (solid blue), test 4 (solid black).

Fig. 4 :

 4 Fig. 4: Norms of the optional contact force f opt,0 (solid blue) and the external force f ext (dotted red) in test 3.

Fig. 5 :

 5 Fig. 5: Position of the CoM along the x axis in test 1 (dashed blue), test 2 (solid blue), test 4 (solid black), and the bounds of the foot support area (dotted red).

Fig. 6 :

 6 Fig. 6: Initial configuration of the robot in test 3.

ACKNOWLEDGMENTS

This work has been funded by the PSPC Romeo 2 project and EU H2020 Comanoid Research and Innovation Action (RIA).