Sébastien Kleff

Ewen Dantec

Guilhem Saurel

Nicolas Mansard

Ludovic Righetti

Introducing Force Feedback in Model Predictive Control

In the literature about model predictive control (MPC), contact forces are planned rather than controlled. In this paper, we propose a novel paradigm to incorporate effort measurements into a predictive controller, hence allowing to control them by direct measurement feedback. We first demonstrate why the classical optimal control formulation, based on position and velocity state feedback, cannot handle direct feedback on force information. Following previous approaches in force control, we then propose to augment the classical formulations with a model of the robot actuation, which naturally allows to generate online trajectories that adapt to sensed position, velocity and torques. We propose a complete implementation of this idea on the upper part of a real humanoid robot, and show through hardware experiments that this new formulation incorporating effort feedback outperforms classical MPC in challenging tasks where physical interaction with the environment is crucial.

I. INTRODUCTION

Many tasks accomplished by humans in everyday life require a sense of touch. For instance feeling external forces is of primal importance when handing over an object, sanding a rough surface or kneading dough. While the importance of haptic feedback in robotic manipulation or locomotion tasks is well acknowledged, recent progress in advanced control methodology based on optimal control have reduced our capability to account for an artificial sense of touch. As a matter of fact, fast numerical optimal control solvers [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF] combined with torque-control capabilities of modern robots have made nonlinear MPC a mature technology for manipulation and locomotion, thanks to its ability to react and reason about the future at the low control level [START_REF] Kleff | High-frequency nonlinear model predictive control of a manipulator[END_REF], [START_REF] Dantec | First order approximation of model predictive control solutions for high frequency feedback[END_REF]. But such controllers reveal their brittleness during contact situations: they rely on simplistic contact models with limited capability to predict future interactions, so the resulting policies are not meaningful. Indeed, when creating contact with an object to fulfill a task, deciding what action to take next should imply some awareness of the force that is currently being applied on that object and how it may evolve in the future.

Consequently these controllers require tedious hand-tuning with no guarantees of success. This fundamental problem remains open for MPC practitioners in robotics: how to control contact interactions ? A common practice is to discard force and torques sensory information in the online optimization and to treat contacts as kinematic constraints. Although this approach circumvents the need for a potentially complex model of interactions with a priori unknown environments, it comes at the price of only being able to control forces in a feedforward sense. Yet there are solid reasons to believe that closing the loop on force measurements inside a MPC framework could increase the robustness and versatility of robots in contact tasks [START_REF] Kim | Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control[END_REF]- [START_REF] Husmann | Model Predictive Force Control in Grinding based on a Lightweight Robot[END_REF]. First, the superiority of MPC over instantaneous control methods in online motion generation is now well established [START_REF] Grandia | Feedback MPC for Torque-Controlled Legged Robots[END_REF], [START_REF] Farshidian | Real-time motion planning of legged robots: A MPC approach[END_REF]. Second, it is known from the field of interaction control that stabilizing a contact interaction requires to take into account both motion and forces [START_REF] Hogan | Impedance control: An approach to manipulation[END_REF]. Thirdly, modern robots are increasingly equipped with diverse sensor modalities which can be exploited, in particular force and torque sensors [START_REF] Kappassov | Tactile sensing in dexterous robot hands -Review[END_REF]- [START_REF] Chawda | Toward torque control of a KUKA LBR IIWA for physical human-robot interaction[END_REF]. In this paper we propose to model the actuation as a low-pass filter in order to allow feedback control on forces.

In the MPC and Trajectory Optimization (TO) literature, contact forces are planned rather than controlled. For instance, standard approaches in locomotion consist in optimizing contact forces as control inputs to the centroidal dynamics [START_REF] Farshidian | An efficient optimal planning and control framework for quadrupedal locomotion[END_REF], [START_REF] Meduri | Biconmp: A nonlinear model predictive control framework for whole body motion planning[END_REF], or as auxiliary variables associated with kinematic constraints of the whole-body dynamics [START_REF] Koenemann | Whole-body model-predictive control applied to the hrp-2 humanoid[END_REF], [START_REF] Posa | Optimization and stabilization of trajectories for constrained dynamical systems[END_REF]. Other works have also proposed to discover online optimized rigid contacts by solving linear complementarity problems [START_REF] Posa | Optimization and stabilization of trajectories for constrained dynamical systems[END_REF], [START_REF] Cleac'h | Fast contact-implicit model-predictive control[END_REF] or smooth contact force models that are compatible with standard TO algorithms [START_REF] Neunert | Whole-body nonlinear model predictive control through contacts for quadrupeds[END_REF]. While these works report experimental evidence of the importance of utilizing predictive models of contact forces to generate contact-rich behaviors, the perceived forces are never fedback and compared against their predictions, which prevents from controlling physical interactions.

From a different perspective, the controller proposed in [START_REF] Grandia | Frequency-aware model predictive control[END_REF] presents similarities with our approach. It enforces the actuation's bandwidth limitation by using a frequencydependent cost function. In that sense, our approach resembles the special case of a low-pass shaping function. But the conceptual difference is that frequency-shaping leaves the relation between state and control unchanged during the optimization, while our controller reasons over higher dimensional dynamics, which enables to naturally derive control policies that depend on torque measurements. The same model augmentation is exploited in [START_REF] Aydinoglu | Stabilization of complementarity systems via contact-aware controllers[END_REF] in order to derive contact-aware policies for complementarity systems.

In another line of work, some authors have proposed to combine nonlinear MPC with interaction control. In [START_REF] Matschek | Force Feedback and Path Following using Predictive Control: Concept and Application to a Lightweight Robot[END_REF] the authors propose an output feedback control strategy combined with path-following MPC in order to track desired forces and motion under nonlinear constraints. In [START_REF] Bednarczyk | Model Predictive Impedance Control[END_REF], MPC is used to achieve a desired impedance while ensuring con-straints. In [START_REF] Minniti | Model predictive robot-environment interaction control for mobile manipulation tasks[END_REF], indirect adaptive control is used to adjust the impedance while interacting with an unknown environment.

In this paper we propose a new MPC paradigm that enables to re-optimize online motions based on measured joint torques. Our approach is based on an augmented dynamic model of the robot that includes an actuation model, and doesn't require any general model of the contact forces. We propose a comparative analysis of performance in simulations and hardware experiments of a sanding task on two torque-controlled robots. We also draw connections with the existing literature of MPC and interaction control, and discuss the implications of this proof-of-concept for the practice of nonlinear MPC on torque-controlled robots.

Our work shows that a simple actuation model suffices to treat forces or torques as controlled variables and that incorporating it into MPC leads to increased performance compared to the classical approach.

II. BACKGROUND

We start the paper by recalling the classical rigid contact model and the consequent classical optimal control problem (OCP) used to control complex robots in contact. Based on this formulation, we will then draw a paradox when trying to feedback on force measurements, which we will use to justify our contribution in the following section.

A. Rigid contact dynamics

A rigid contact is a kinematic constraint acting at the point of contact between the robot and the environment. The equations of motion of a fully-actuated robot in rigid contact with the environment can be derived from the Karush-Kuhn-Tucker (KKT) conditions of the convex optimization problem corresponding to Gauss' least constraint principle [START_REF] Udwadia | A new perspective on constrained motion[END_REF]

min v 1 2 ∥ v -vf ∥ 2 M (q) (1a) s.t. J(q) v + J(q)v = 0 (1b)
where q, v ∈ R n are the vectors of joint positions and velocities, M (q) ∈ S n + is the generalized inertia matrix, J(q) ∈ R 6×n is the contact Jacobian, vf = M (q) -1 (τh(q, v)) ∈ R n is the free acceleration, h(q, v) ∈ R n is the vector of centrifugal, Coriolis and gravity forces and τ ∈ R n is the vector of joint torques. As explained in [START_REF] Budhiraja | Differential dynamic programming for multi-phase rigid contact dynamics[END_REF] the generalized contact forces λ ∈ R 6 appear in the KKT conditions of (1) as the Lagrange multiplier associated with the rigid contact constraint (1b). The solution to the KKT system has the form of a constrained dynamical system with state x ≜ (q, v) and control u ≜ τ

ẋ = f (x, u) (2a) λ = g(x, u) (2b)

B. Classical MPC formulation

The following OCP is solved online

min u(.),x(.) T 0 l(x(t), u(t), λ(t), t)dt + l T (x(T )) (3) s.t.                ẋ(t) = f (x(t), u(t)) λ(t) = g(x(t), u(t)) x(0) = x ini x(t) ∈ X u(t) ∈ U
where x ini is the initial state, l, l T the running and terminal costs, X , U represent state and control constraints. We transcript this OCP into the following nonlinear program (NLP)

min X,U N -1 k=0 L k (x k , u k , λ k ) + L N (x N ) (4)
s.t.        x k+1 = F (x k , u k ) λ k = G(x k , u k ) x 0 = x ini
where L, L N , F, G are the discretized cost and dynamics with sampling step ∆t. The inequality constraints are handled by penalization in the cost. We use the FDDP algorithm implemented in [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF] to solve (4). The algorithm outputs a locally optimal control policy of which we only apply the first element to the measured state x

π * 0 (x) = u * 0 + K 0 (x -x * 0 ) (5)
The feedback gain K 0 compensates small deviations around x * 0 and can serve to interpolate the control trajectory between two MPC updates as the control rate usually is higher than the update rate. For more details the reader can refer to [START_REF] Dantec | First order approximation of model predictive control solutions for high frequency feedback[END_REF].

C. Rigid contacts accept no predictive feedback

In this context, explicit force feedback is not possible because contact forces and control torques are algebraically coupled. While λ is used as a prediction, it is possible to choose a control action u. But if λ is used as a measurement (like the state x), it can be seen from (2b) that u is completely determined and cannot be chosen. In fact, λ appears as the output of a nonlinear system in with instantaneous transfer from the input, since the feedthrough

∂ λ ∂u = (JM -1 J T ) -1 JM -1 is non-zero.
In this context, attempting to control λ with some policy u(λ) would create an algebraic loop as u and λ would influence each other instantaneously. One way to be able to break this loop is to introduce delay between force and control input as suggested in [START_REF] Aydinoglu | Stabilization of complementarity systems via contact-aware controllers[END_REF]. Another way is to model the contact as non-rigid as proposed e.g. in [START_REF] Matschek | Force Feedback and Path Following using Predictive Control: Concept and Application to a Lightweight Robot[END_REF]. We follow [START_REF] Aydinoglu | Stabilization of complementarity systems via contact-aware controllers[END_REF] and introduce delay by modeling the actuation abstractly.

III. FORCE FEEDBACK MPC

In this section, we formulate the augmented dynamics and OCP. The MPC scheme with force feedback is then introduced.

A. Augmented dynamics

The actuation dynamics is modeled as low-pass filter (LPF). The command torque w is filtered into an actuation torque τ according to

τ = ω c (w -τ ) (6)
where ω c > 0 is the cut-off angular frequency in rad s -1 , i.e. ω c = 2πf c where f c is the ordinary cut-off frequency in Hz. The augmented robot dynamics reads

ẏ ≜   q v τ   =    v M -1 (τ -h + J T λ) ω c (w -τ )    (7)
where y ≜ (q, v, τ ) denotes the augmented state of the robot including the classical state x = (q, v) and the filtered torques τ , and w is the new control input corresponding to the unfiltered torque, that is torque reference sent to the motor. The discretized actuation dynamics is an exponential moving average (first order infinite impulse response filter)

τ k+1 = ατ k + (1 -α)w k ( 8
)
where α is related to cut-off frequency and discretization step ∆t. Applying a zero-order hold in the Laplace domain on the CT filter

H(s) = ω c s + ω c (9)
leads to the exponential formula α = e -2πfc∆t . Note that f c → ∞ corresponds to the case where there is no filtering, i.e. the actuator is perfect and w goes entirely through (α → 0). On the opposite, f c → 0 corresponds to an infinite filtering effect, i.e. w is fully blocked (α → 1).

While the actuation model introduced here is simple and abstract -a single scalar and a linear equation express the actuation bandwidth and delay -we will show that it led to good experimental results. In fact, this model is in some sense generic as it should capture the linear behavior of most other actuation models. Besides, the proposed method goes beyond this abstract model, and other actuation models could be considered, which we will investigate in the future.

B. Augmented OCP

The new optimal control problem yields min w(.),y(.) T 0 l(y(t), w(t), λ(t), t)dt + l T (y(T ))

s.t.                      ẋ(t) = f (x(t), τ (t)) τ (t) = ω c (w(t) -τ (t)) λ(t) = g(x(t), τ (t)) y(0) = y ini y(t) ∈ Y w(t) ∈ W (10)
Fig. 1: MPC scheme with augmented dynamics. The measured torque is injected into the MPC and an interpolation of the optimal torque is sent to the actuators. which is transcripted using the same method as for (4), using an explicit integration scheme for the augmented state

min Y,W N -1 k=0 L k (y k , w k , λ k ) + L N (y N ) (11)
s.t.            x k+1 = F (x k , τ k ) τ k+1 = ατ k + (1 -α)w k λ k = G(x k , τ k ) y 0 = y ini (12)

C. MPC with force feedback

The numerical solver (FDDP [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF]) computes optimal state and control sequences Y * = {y * 0 , ..., y * N } W * = {w * 0 , ..., w * N -1 } as well as Riccati feedback gains K k defining a locally optimal stabilizing policy around each shooting node

π k (ŷ) = w * k + K k (y k -ŷ) (13)
We recall that y = (q, v, τ ) gathers all joint measurements (see Figure 1). At each MPC cycle, the initial state of the OCP is set to the measured state y 0 ← ŷ. We could be tempted to apply the unfiltered optimal torque w * 0 but it seems safer to send a filtered torque that represents our "best" available reference (the torque we actually want the actuator to produce). Indeed, if the LPF model is not accurate the optimizer can overestimate the filtering effect and pick an overly aggressive torque during the optimization.

Considering that the filtered torque is used as the actual control input, we need to be careful not to send the last measured torque as the current command to the robot. Indeed we have τ * 0 ← τ at each MPC cycle (see Figure 1). Furthermore, it may not be satisfactory either to use the prediction τ * 1 which lies too far in the future if the OCP sampling rate ∆t is greater than the MPC update rate ∆t M P C -note that selecting ∆t > ∆t M P C is common practice and allows more flexibility, e.g. in trading off numerical integration accuracy against horizon length while keeping a fast update rate. In fact it makes more sense to use a linear interpolation of the optimal filtered torque :

τ = τ * 0 + ϵ(τ * 1 -τ * 0 ) (14)
where ϵ = ∆t M P C ∆t . As shown in [START_REF] Dantec | First order approximation of model predictive control solutions for high frequency feedback[END_REF], the Riccati gain K 0 computed by DDP correspond to the first-order derivatives of the optimal control with respect to the initial state, i.e. K 0 = ∂ w * 0 ∂y 0 . This property can be used to interpolate the MPC policy at higher frequencies. In our case, the locally optimal feedback gain associated with control [START_REF] Farshidian | An efficient optimal planning and control framework for quadrupedal locomotion[END_REF], namely K ≜ ∂ τ ∂y 0 , reads

K = ∂ τ * 0 ∂y 0 + ϵ( ∂ τ * 1 ∂y 0 - ∂ τ * 0 ∂y 0 ) (15)
Noticing that ∂ τ * 0 ∂x = 0 and ∂ τ * 0 ∂τ = I, and using ( 8), the position-velocity gains ( Kx ) and torque gains ( Kτ ) can be can then be expressed in terms of K 0

Kx = ϵ(1 -α)K x 0 (16) Kτ = I + ϵ(1 -α)(K τ 0 -I) (17)
and the policy ( 14) can be interpolated at the control rate as

π(ŷ) = τ + Kx (x 0 -x) + Kτ (τ 0 -τ ) (18)

IV. SIMULATION ON A MANIPULATOR

In this section we compare the classical MPC and the force feedback MPC in simulation with the 7-DoF KUKA LBR iiwa. The task consists in drawing a circle on a flat horizontal surface (10 cm-radius, 1.5 rad s -1 ) while exerting a constant normal force (20 N). The contact model used here is a 1-D contact model that constrains the robot motion only along the direction normal to the contact plane.

A. Sanding task formulation

The sanding task includes state and torques regularization and soft limits, an end-effector force and position tracking, and a frame orientation objective.

1) Classical MPC: The cost function in (3) is

l(x, u, λ) = c x ∥A x (x -x 0 )∥ 2 + c u ∥A u (τ -τ g (q))∥ 2 + c λ ∥A λ (λ -λ)∥ 2 + c R ∥A R (R(q) ⊖ R)∥ 2 + c p ∥A p (p(q) -p(t)∥ 2 (19)
where p, R are the end-effector frame 3D position and rotation respectively, τ g (q) is the gravity torque, A x , A u , A p , A λ , A R are activation weight matrices, c x , c u , c p , c λ , c R are scalar costs weights, ⊖ represents the difference in SO(3).

2) Force feedback MPC: The cost function in [START_REF] Kappassov | Tactile sensing in dexterous robot hands -Review[END_REF] is

l(y, w, λ) = c y ∥A y (y -y 0 )∥ 2 + c lim y ∥B y (y)∥ 2 + c λ ∥A λ (λ -λ)∥ 2 + c R ∥A R (R(q) ⊖ R)∥ 2 + c p ∥A p (p(q) -p(t))∥ 2 + c w ∥A w w∥ 2 + c lim w ∥B w (w)∥ 2 (20)
A y , A w are activation weight matrices on the augmented state and unfiltered torque respectively, c y , c lim y , c w , c lim w are scalar costs weights, B y , B w are weighted quadratic barriers.

B. Imperfect actuation and perfect contact model

The PyBullet simulation environment doesn't allow the simulation of low-level actuation. In order to simulate a sensible torque measurement, we propose to simulate actuation and sensing uncertainty as follow

τ (t) = aτ (t -δ o ) + b + η τ (21a) x(t) = x(t -δ s ) + η x (21b)
where Each controller was tuned separately to achieve its best performance, to the best of our ability. We found that MPC parameters were very dependent on the controller. Eventually, the classical MPC frequency was set to 500 Hz with ∆t = 20 ms and N = 50 nodes (1 s horizon), while the force feedback MPC frequency was set to 250 Hz with ∆t = 10 ms and N = 100 nodes (1 s horizon), and the cutoff frequency is set to f c = 5 Hz 1 In both cases, the simulation frequency is 1 kHz and a maximum of 5 DDP iterations is allowed at each MPC update. The policies used are (5), [START_REF] Cleac'h | Fast contact-implicit model-predictive control[END_REF].

η x ∼ N (0, σ x ), η τ ∼ N (0, σ τ )
We choose as performance metric the average position tracking error, the average normal force error, the peak normal force and the percentage of the simulation time spent not in contact, as reported in Table I. We observe a higher tracking performance in position and force in the case of the force feedback MPC. Also the proposed controller results in a lower maximum force, and maintains contact with the environment throughout the task.

C. Imperfect actuation and disturbed contact model

In order to assess the benefit of torque feedback, we compare the performances when the contact model is disturbed and under different actuation models. The contact surface is tilted about the y-axis by an angle ranging from -20 • to +20 • , and for 5 different actuation models (i.e. random pairs of (a, b) in ( 21)). The results are shown in Figure 2. The position and force tracking performances are better with the torque feedback MPC. Moreover, the maximum force is lower than with classical MPC (about 50 N vs 220 N for the 20 • angle), which can be explained by the more frequent contact breaking: about 40% of the simulation time is spent not in contact without torque feedback vs less than 10% for the proposed approach. Also, the standard deviation of position and force performance over actuation models is smaller (i.e. with fixed tilt angle), which suggests that our approach is less sensitive to actuation model uncertainty.

V. EXPERIMENT ON A HUMANOID

We propose now to validate the approach with hardware experiments of the sanding task on the torque-controlled TALOS humanoid robot [START_REF] Stasse | Talos: A new humanoid research platform targeted for industrial applications[END_REF] with a perturbed contact model.

A. Experimental setup

We use the same ROS-based real-time control architecture as [START_REF] Dantec | First order approximation of model predictive control solutions for high frequency feedback[END_REF]. The MPC runs at 100 Hz with 3 DDP iterations. The OCP is solved using the Crocoddyl library [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF] and rigidbody computations are done using Pinocchio [START_REF] Carpentier | The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives[END_REF]. The lower level is controlled at 2 kHz. The robot model is reduced to a 6-dimensional model including the torso and right arm. All other joints are position-controlled in a fixed posture. A soft material is taped to the robot forearm to damp impacts. Torque measurements are filtered with a moving average.

At the time of the experiments, FT sensors in the wrist were not available so we used joint torque measurements to estimate the contact force

λ λ = ( Ĵ M -1 ĴT ) -1 (γ -Ĵ M -1 (τ -ĥ)) (22)
where the spatial acceleration γ is neglected. This estimation is not accurate enough to legitimate a thorough quantitative analysis of performance as proposed in the previous section. Therefore we leave this for future work and restrict ourselves to a qualitative discussion for now, which is nevertheless relevant for this proof of concept.

B. Sanding task

The general cost function used for both task is similar the cost functions ( 19) and ( 20) used in simulation, the only difference being the introduction of time phases: the main task is divided into sub-tasks with fixed time duration (posture regularization phase, table reaching phase, contact stabilizing phase, circle tracking phase, etc.). Hence the cost weights c p , A p , c R , c λ and the rigid contact constraint activeness are made time-varying. Also for each phase switch, the OCP parameters is updated in a receding horizon fashion, i.e. progressively starting from the last node in the horizon -it empirically led to more stability than updating all the nodes at once.

C. Results

The robot must draw 7 cm-radius circles at 1 rad s -1 on a table. The contact model is assumed to be perfect by the controller (perfectly horizontal table) but the real table is tilted by an unknown angle. As in simulation, each controller was tuned separately to the best of our abilities. The Riccati torque feedback gains derived in [START_REF] Cleac'h | Fast contact-implicit model-predictive control[END_REF] were not used for the force feedback MPC since TALOS already has a low-level torque control loop running with fixed feedback gains. For both the classical and proposed controller, the Riccati gains on position and velocity are used to interpolate the MPC solution at the control frequency.

As seen in the supplementary video, the motions generated by the force feedback MPC are smoother than with the classical controller. Our controller maintains contact throughout the whole task while the classical controller often breaks contact, creates more vibrations with the table. As seen on Fig. 3a,3b, the force variations have a greater amplitude in the case of classical MPC. The predictions of the classical MPC (green) are perfect, which explains the large mismatch between actual and predicted force. Our controller predicts forces that match more closely the actual ones thanks to its force feedback, which explains the observed improved performance. Fig. 4a,4b show that the circle tracking is improved with the force feedback MPC, while the classical controller struggles to track the circle properly because of frequent contact breaking. We couldn't achieve a higher accuracy with the classical MPC as increasing the position gains led to instability.

VI. CONCLUSION

A. Discussion 1) Contact force measurements: We have used joint torque sensors as feedback but we further we intend to use Cartesian force measurements as proposed in [START_REF] Matschek | Force Feedback and Path Following using Predictive Control: Concept and Application to a Lightweight Robot[END_REF] and retrieve the measured torques as τ = M v + h -J T λ. One interesting challenge to overcome is the excitation of the robot structural dynamics and the apparition of coupled instabilities due to the non-collocation of sensor and actuators. This will pave the way for a more general inclusion of force sensing in MPC.

2) Riccati feedback gains: TALOS already has a low-level torque control loop with fixed feedback gains. But as Section III-C shows, our approach naturally provides locally optimal feedback gains on the torques. Future work therefore includes an empirical evaluation with these gains to replace other lowlevel controllers and investigate how this improves tracking performance.

3) Improved actuation model: A simple LPF already shows performance improvement compared to classical MPC, therefore we could expect even better results with a more sophisticated actuation model.

B. Controlling physical interaction

We draw here a parallel with the existing interaction control literature. During physical interaction, motion and forces are coupled through the exchange of mechanical work [START_REF] Fasse | Control of Physical Contact and Dynamic Interaction[END_REF]. An important consequence is that controlling an interaction requires to regulate force and motion dynamically. When the mechanical energy is small enough this coupling can be neglected but in many interesting cases it cannot (e.g. deforming an object, scraping, etc.).

With this in mind, impedance control [START_REF] Hogan | Impedance Control Part1-3[END_REF] aims at modulating the robot reaction to physical interaction by regulating the dynamic relation between force and motion. While high impedance robots have been historically predominant, in part for technological reasons, low impedance is required for many tasks [START_REF] Pratt | Low Impedance Walking Robots[END_REF]. Explicit force feedback is an efficient way of doing so because it scales the apparent inertia of the robot [START_REF] Volpe | The equivalence of second order impedance control and proportional gain explicit force control: Theory and experiments[END_REF]. But it can render the system non-passive and prone to coupled instabilities [START_REF] Fasse | Control of Physical Contact and Dynamic Interaction[END_REF], [START_REF] Whitney | Historical perspective and state of the art in robot force control[END_REF]. Therefore passivity is classically used as a sufficient criteria for stability [START_REF] Fasse | Control of Physical Contact and Dynamic Interaction[END_REF], [START_REF] Colgate | The Control of Dynamically Interacting Systems[END_REF]- [START_REF] Focchi | Robot impedance control and passivity analysis with inner torque and velocity feedback loops[END_REF]. This condition limits the magnitude of the force gains, and thereby the achievable bandwidth [START_REF] Eppinger | Understanding bandwidth limitations in robot force control[END_REF], [START_REF] Dohring | The passivity of natural admittance control implementations[END_REF] which can be overly conservative: some tasks may require precisely behaviors that are stable but not necessarily passive (e.g. deforming an object, grinding).

This observation raises the following question: what particular impedance should a robot realize in order to execute a given task? Note that impedance control is agnostic to this "inverse" impedance problem. In practice, designing a suitable impedance is done empirically through expert finetuning -which is tedious and vulnerable to uncertainties. While we are not claiming to provide a formal solution to this problem, we like to think of optimal control as a way to automatize control gains synthesis, e.g. by relating impedance modulation during contacts to a trade-off between disturbance rejection and measurement uncertainty [START_REF] Hammoud | Impedance optimization for uncertain contact interactions through risk sensitive optimal control[END_REF]. This suggests that incorporating force in the optimization may result in an optimized impedance, trading off motion and force performance to achieve a higher-level objective.

C. Summary and future prospects

We proposed a novel paradigm to exploit force measurements in MPC. By introducing an actuation dynamics between desired torques and actual torques, we were able to allow force feedback in optimal control which led to a significant improvement in performances for contact tasks compared to classical state-based MPC. We demonstrated the benefit of this new approach through simulations and hardware experiments. This proof of concept confirms our intuition that MPC should tend toward multimodality by including more sensors. We intend to continue investigating this subjet in the future.

  are Gaussian noise signals capturing sensing and actuation noise, a, b are scaling coefficients uniformly drawn from [a, ā] × [b, b], t is the simulation time, δ o is a delay due to the computation time of the MPC and δ s is a delay introduced by e.g. transmission or sensing. We set a = 0.95, ā = 1.05, b = -0.1 N m, b = 0.1 N m δ o = 1 ms, δ s = 2 simulation cycles.

Fig. 2 :

 2 Fig. 2: Performance on the sanding task of classical MPC (blue) and force feedback MPC (red) for a range of tilting angles averaged over several actuation models. The proposed controller achieves higher performance in position and force.

Fig. 3 :

 3 Fig. 3: Normal force during the sanding task with perturbed contact model. The robot maintains contact despite perturbations with force feedback MPC (bottom plot) whereas it breaks contact several times with classical MPC (top plot).

Fig. 4 :

 4 Fig. 4: The position tracking is more accurate with the force feedback MPC (bottom) than with the classical controller (top) thanks to proper control of contact interaction through force and motion feedback.

TABLE I :

 I Performance on simulated sanding task with KUKA iiwa -imperfect actuation, perfect contact model

Tandon School of Engineering, New York University, Brooklyn, NY

LAAS-CNRS, Université de Toulouse, CNRS, Toulouse

Artificial and Natural Intelligence Toulouse Institute (ANITI), Toulouse

Max Planck Institute for Intelligent Systems, Tübingen We would like to thank Pierre Fernbach for his valuable support during hardware experiments.

This value, also used in the hardware experiments, was identified experimentally on TALOS actuators.