Optimal Energy Tank Initialization for Minimum Sensitivity to Model Uncertainties

Energy tanks have gained popularity inside the robotics and control communities over the last years, since they represent a formidable tool to enforce passivity (and, thus, input/output stability) of a controlled robot, possibly interacting with uncertain environments. One weak point of passification strategies based on energy tanks concerns, however, their initialization. Indeed, a too large initial energy can cause practical unstable behaviors, while a too low initial energy level can prevent the correct execution of the task. This shortcoming becomes even more relevant in presence of uncertainties in the robot model and/or environment, since it may be hard to predict in advance the correct (safe) amount of initial tank energy for a successful task execution. In this paper we then propose a new strategy for addressing this issue. The recent notion of closed-loop state sensitivity is exploited to derive precise bounds (tubes) on the tank energy behavior by assuming parametric uncertainty in the robot model. These tubes are then exploited in a novel nonlinear optimization problem aiming at finding both the best trajectory and the minimal initial tank energy that allow executing a positioning task for any value of the uncertain parameters in a given range. The approach is finally validated via a statistical analysis in simulation and experiments on real robot hardware.


I. INTRODUCTION
Passivity-based control (PBC) has gained large attention over the past years, especially in the robotics field.One of the main reasons behind this great success are related to the physical interpretability of PBC and to its modularity.Indeed, the controller and the robotic system are usually equivalent to a physical system.This is the case of the well-known mass-spring-damper in impedance/admittance control [1], or distributed mass-spring system in communication channels [2].Thanks to these characteristics, the resulting behavior of the system is easier to understand and the implementation is more intuitive.Moreover, coupling a passive system with another passive system leads to a coupled system that is still passive, making passivity-based control really modular.Passivity is a significant property since a passive system is not only simply stable, but passivity is a necessary and sufficient condition for achieving robust stability [3].Passivitybased control has been exploited in many robotic fields, such as collaborative robotics [4], bilateral teleoperation [5], and multi-robot system [6].
However, traditional passivity-based control may lack flexibility.By selecting a specific passive dynamics, indeed, the behaviour of the system may be limited and too much constrained.For example, in human-robot collaboration tasks impedance adaptation is crucial to achieve a good interaction [7].Likewise, in teleoperation scenarios it may be necessary to change the stiffness of the coupling when the environment changes [8].Lastly, in case of multi-robot system, it may be necessary that some of the agents implement a reactive behaviour which cannot be guaranteed by a fixed coupling [9].
In all these (and similar) examples, energy tanks [10] have been introduced for decoupling passivity-based control from the specific dynamics of the system.Energy tanks are virtual elements that can store the energy naturally dissipated by the system.This energy can then be reused for, e.g., implementing a desired behavior in a (guaranteed) passive way.The main advantage of energy tanks is their intrinsic flexibility, which can allow to easily enforce passivity and, therefore, robust stability for virtually any task and robot/environment of interest.
One of the main open points in the use of energy tanks is related to their initialization.In fact, even if formally passive, a system passified via an energy tank can still have practical unstable behaviors if the initial tank energy is too high [6].On the other hand, if the tank is initially not loaded with sufficient energy, the system may not be able to perform the desired task.In [11], [12] the authors introduced the concept of Task-energy, which is the energy required for realizing a force tracking task.By exploiting this method it is then possible to estimate in a non-heuristic way the initial energy for the tank.In [13] the concept of frozen energy is introduced as an extra-energy reservoir to be released into the tank when necessary for completing the task.In [14], the tank is initialized with a high amount of energy and a strategy for incrementally making some energy available based on and online estimation of the task to be executed is proposed.
These approaches, however, are not robust to uncertainties in the robot/environment, in particular in their parameters such as mass, friction, stiffness and so forth.Therefore, if the real model parameters differ from the nominal ones used to estimate the task-energy, the tank may still deplete (or alternatively one would need to overestimate the initial tank energy, therefore defeating the purpose of a principled initialization).
Explicitly considering the role of model uncertainties in planning/control design is of paramount importance in robotics.A lot of work has been done in the past literature to address and overcome uncertainties of the robot model and/or the environment.A possible approach is to estimate online the uncertain parameters for adjusting the control action during the task execution [15].However, parameter estimation often requires some form of 'persistency of excitation' in the robot trajectories, which may not always be practical or attainable for the task at hand.Furthermore, online parameter estimation may lead to undesired behaviours such as erratic/unpredictable transients.Another popular possibility is to instead generate feedforward (planned) trajectories for minimizing the effects of uncertainties [16]- [20].These works are, however, often either limited to the open-loop cases (without taking into account the controller), or require a very specific control design (which may not always be desirable, computationally feasible or even attainable for non-trivial systems).An alternative approach has been recently developed in [21]- [24] by introducing and exploiting the notion of closed-loop state sensitivity for a robot/controller pair: in short, for any robot/environment model and control strategy, it is possible to compute the socalled state sensitivity matrix Π that relates how variations in the model parameters affect the robot states in closed-loop.Furthermore, by assuming known ranges for the (otherwise unknown) uncertain parameters, it is also possible to exploit Π for evaluating the tubes of perturbed states/inputs along a future trajectory [23].These quantities can then be leveraged for producing motion plans that are intrinsically minimally sensitive to parametric uncertain, and with a robust and guaranteed satisfaction of constraints such as limited actuation or obstacle avoidance, see [23], [24] for some examples.
Inspired by this line of works, the aim of this paper is to explicitly consider presence of parametric uncertainties in the robot/environment model during an interaction task passified by an energy tank, and to then study how the sensitivity machinery can be exploited for finding a suitable initialization of the tank energy (despite the model uncertainty).Summarizing, the main contribution of this work are: • The definition of the closed-loop sensitivity of the energy tanks; • The definition of a new optimization problem that exploits the sensitivity for optimally initializing the energy tank, i.e., choosing the lowest initial tank energy that still ensures realization of the task in presence of model uncertainties; • A comprehensive validation of the proposed approach, both in simulation and in a real scenario.
The paper is organized as follow: in Sec.II the problem related to the initialization in presence of uncertainties is detailed, while Sec.III provides the main background related to the sensitivity.In Sec.IV, instead, the sensitivity notions are applied to the energy tanks and the sensitivity of the tank state is derived.This concept is exploited in Sec.V to build an optimization problem that allows to initialize the tank in an optimal way.Lastly, in Sec.VI, an extensive validation carried out both in simulation and in a real scenario is provided, while future works are addressed in Sec.VII.

II. PROBLEM STATEMENT
Consider a fully-actuated gravity compensated robotic system modeled in the task space by the following Euler-Lagrange equation: where x, ẋ, ẍ ∈ R nx are the pose, the velocity and the acceleration of the robot, respectively.M (x), D(x) ∈ R nx ×R nx and the positive definite inertia matrix and the positive semidefinite damping matrix respectively.C(x, ẋ), instead, represents the Coriolis terms.Finally u ∈ R nx , F e , ∈ R nx are the control input and the external force due to the interaction with the environment and y = ẋ, ∈ R nx is the output of the system.As well known, see e.g.[25], (1) is passive with respect to the pair (u + F e , y) using the kinetic energy ẋT M (x) ẋ as a storage function.Consider the task of generating a desired reference trajectory x d (t) ∈ R nx , with t ∈ t i , t f that takes (1) from an initial configuration x(t i ) to a final configuration x(t f ) and consider any desired controller that generates a control input u d (t) such that e(t) = x d (t)−x(t) → 0 with a desired transient in case of free motion, i.e.F e = 0.If the controlled system is passive, then it is robustly stable [3], [6] and it would not be destabilized in case of interaction with the enrvironment, i.e.F e ̸ = 0. Trajectory tracking controllers do not ensure a passive behavior by design, but passivity can be implemented using energy tanks [10].
Energy tanks are virtual energy storing elements that can exchange energy through their input/output port and they are represented by where x t ∈ R is the state of the tank and (u t , y t ) ∈ R × R is the power port of the tank.The energy stored in the tank is given by and, by combining (2) with (3) it is straightforward to show that the tank can store/release the energy through its power port: Energy tanks can be interposed between the plant (1) and the controller in order to ensure passivity, as shown in Fig. 1 (see, e.g., [6] for more details).
Exploiting the output y of the system and any other measurable signal, the controller generates the desired control input u d that is exploited for modulating the interconnection between the tank and (1).The energy tank is joined to (1) as follows: where w = u d yt = u d xt , with u d the desired input of the system, and D = ẋT D(x) ẋ is the amount of energy dissipated by the system.Using (5), the energy tank stores System Energy Tank Controller Fig. 1: Control scheme the energy dissipated by (1) and, through the modulation gain w, exploits the stored energy for reproducing (in a passive way) the desired input as long as sufficient energy is present in the tank.In order to avoid singularity problems it is necessary to initialize the tank with enough initial energy and to prevent depletion of the stored energy over time.This can be achieved by initializing the tank with a non-zero initial value, i.e. x t (t i ) ̸ = 0, and by introducing a modular function σ(t) such that: where ε is a design lower threshold.By plugging ( 6) in ( 5) it is possible to obtain the modulated power port: As shown in [6], the control scheme in Fig. 1 is passive with respect to the pair (F e , y) using the storage function K(t) + T (t), independently of the value of u d .The desired input generated by the controller can be implemented only if sufficient energy in the tank is available and, consequently, possibility of tracking a trajectory in t ∈ t i , t f depends on the amount of energy stored in the tank.
Initializing the energy tank with a very large amount of energy would easily solve the problem and would lead to a theoretically passive but practically unstable behavior [6].The approaches available in the literature for initializing the tank consider a specific set of parameters characterizing the system and the controller and, therefore, its robustness and practical usability are not optimal.Furthermore, uncertainties in the robot/environment model are also not explicitly considered, therefore further reducing the usability in many real scenarios.The aim of this work is therefore to propose an integrated and novel optimization problem such that, for any desired controller/task and by explicitly considering presence of parametric uncertainties for the robot/environment models, achieves the following goals: • generation of an optimized trajectory x d (t) for the chosen robot/controller pair guaranteed to realize the task without depletion of the tank energy for any value of the uncertain parameters in a given range; • 'optimal' initialization of the tank energy, i.e., the smallest possible initial energy that ensures a correct task execution despite the uncertain parameters.

III. SUMMARY OF STATE SENSITIVITY NOTIONS
Consider a generic robot/environment model where q ∈ R nq represents the robot state, u ∈ R nu is the control input, and p ∈ R np represents the (possibly uncertain) robot/environment parameters.For instance in manipulation and interaction tasks one may consider uncertainties in the mass or inertial properties of the manipulated object, in the stiffness/friction parameters of the environment, and so forth.
In the context of tracking tasks, let us consider an output function y(q) ∈ R ny (e.g., the manipulator pose) that needs to follow a desired reference trajectory y d (t) ∈ R ny , and an associated trajectory tracking controller with the generic form ξ = g(ξ, q, y d (t), p c , k c , t), where ξ ∈ R ξ are the internal states of the controller (e.g. an integral action), p c ∈ R np are the nominal model parameters, i.e., the nominal value of p used by the controller that may differ from the real p, and k c ∈ R kc are the controller gains.
For the closed-loop system (8-9) it is possible to define the so-called state sensitivity matrix and input sensitivity matrix which quantify how variations in the parameters p (w.r.t. the nominal p c ) affect the states and inputs during motion.A closed-form expression for Π(t) and Θ(t) is not available in the general case, but in [21], [23] it is shown how these quantities can be easily obtained by forward integrating the following system of differential equations where Π ξ ∈ R ξ×np is the sensitivity of the controller states.Matrices Π(t) and Θ(t) can be used for several purposes such as optimization of the reference trajectory y d (t) for producing minimally sensitive motion plans (by minimizing some combined norm of Π(t) and Θ(t) as done in [21], [22]).An extension has also been recently proposed in [23] for evaluating the tubes (or ellipsoids) of uncertain states/inputs.Assume that each parameter p i can vary in a given range δp i centered at a nominal p ci and define the diagonal weight matrix W = diag(δp 2 i ).Letting ∆p = p − p c , an ellipsoid in parameter space centered at p c and with semi-axes δp i has equation and, as discussed in [23], one can obtain the corresponding ellipsoids in state space and in input space Here ∆q stands for ∆q = q − q nom where q nom is the state evolution of (8-9) in the unperturbed case p = p c , and analogously for ∆u = u − u nom .
The state and input space ellipsoids can be used for different purposes.In particular, in the context of this work, one can exploit (15)(16) for obtaining the tubes of perturbed trajectories for the individual components of the states and the inputs.By again referring to [23] for all details, for each direction of interest in the state space one can obtain the 'tube radius' r i (t) such that where, as usual, q nom,i (t) is the behavior of the state q i (t) in the unperturbed case p = p c .Equation ( 17) bounds from above/below the envelope of perturbed states when the parameter uncertainty is bounded as in (13), and an analogous upper/lower bound can also be obtained for the inputs components u i (t).

IV. SENSITIVITY OF ENERGY TANKS
Exploiting the sensitivity framework detailed in Sec.III, it is interesting to analyze how uncertainties in the parameters p would affect the state of the tank.However, the sensitivity framework is based on the assumption that all the equations involved in the control are continuous and differentiable, which is not the case for the standard tank modulation (6).Therefore, in this paper a new modulation function is introduced: where the step function is approximated with a sigmoid function.
At this point, it is possible to define the tank sensitivity as: which represents how sensitive the state of the tank is w.r.t. the variations of the parameters around p c .To improve the overall behavior of the system, it is possible to analyze how the uncertainties in the parameters affect the state of the tank in order to avoid its depletion, i.e. the output of the tank u will be equal to the desired controller output u d .Following analogous derivations as in (12), one obtains where Π y is the row-block of matrix Π associated to the sensitivity of the system output (the system velocity in our case).Equation (20) allows to analyze the behaviour of the tank from a different perspective, i.e., from the variations of the parameters.First of all, it is possible to note that also the tank sensitivity suffers singularities at x t = 0. Thus, the lower bound of the tank ε affects the sensitivity of the tank, i.e. small values of ε would make the tank more sensitive to the variation of the parameters.However, as demonstrated in [11], it is possible to choose ε at will, since it is only necessary that the initial difference T (t i ) − ε is enough to execute the task.Secondly, it is possible to note that when the system dissipates a large amount of energy D the sensitivity Π xt decreases, i.e. when the i-th component of Π xt is positive the damping D would reduce its value and when it is negative the damping would increase its value.Intuitively, this means that in the presence of an high dissipation, the state of the tank is more robust to the variations of the parameters.However, the variation of the damping w.r.t. to the state x may still "negatively" affect the robustness of the tank, since it depends on the sign of Π y and ∂D ∂x .It is worth underlining that the amount of energy injected in the tank D is defined by construction of (7).Thus it is possible to have ∂D ∂x = 0.
As already done for the other sensitivity matrices in Sec.III, it is possible to build the kernel matrix of the ellipsoid for the tank state: which in our case is simply a scalar.The radius r xt (t) of the tube around the tank state is then: which leads to the following tube for the tank state in presence of parametric uncertainties: where x t,c is, as usual, the evolution of the tank state when p = p c .

V. OPTIMIZATION PROBLEM
We now show how the tank sensitivity introduced in Sec.IV can be exploited for optimization purposes.The main goal is to ensure that, even in presence of parametric uncertainty, the system will be able to execute the task and the tank will never deplete.This can be mathematically expressed by the following inequality: The tank sensitivity Π xt , however, refers to the internal state of the energy tank and not to actual stored energy.To this aim, it is possible to express the inequality (24) as function of x t : where only the solution x t > 0 is admitted.Considering also the uncertainty tube in (23) it is possible to find a lower bound for the tank state: Starting from the constraint (26), it is then possible to define which is the optimal initial tank state x t (t i ), i.e. the lowest initial energy, and the best trajectory x d (t) such that the system will be able to perform the task with the desired controller and the energy tank will not deplete for any value of the uncertain parameters in the assumed range of variation.This can be obtained by solving the following optimization problem: where a ∈ R na is a vector of parameters for shaping the desired trajectory x d (a, t) (e.g. the coefficients of a polynomial trajectory), b eq ∈ R n b is the vector of the boundary conditions while A eq ∈ R n b ×na is the matrix that allows matching these conditions.The first constraint guarantees that the optimized trajectory is compliant with the desired boundary conditions, e.g., initial/final position, velocity and acceleration.The second constraint ensures that the tank will not deplete despite the parametric uncertainty.The third constraint is used to ensure that the energy inside the tank will not exceed an admissible threshold T max .By imposing this threshold, it is possible to guarantee that the behavior of the robot will remains stable also in practical cases.
As already mentioned in [21], the concept of sensitivity and the uncertainty tubes may also be exploited to find a trajectory that ensures the maximum robustness to the variations of p. Thus, if necessary, the cost function of the optimization problem in (27) may also be modified as follows: • 1 2 ∥Π xt (t f )∥ 2 for reducing the sensitivity at the end of the trajectory.
ti ∥Π xt (τ )∥ 2 dτ for reducing the sensitivity along the whole trajectory.Of course, one could also consider optimization of the sensitivity of the states and/or inputs as done in [21], [22].It is worth noting that choosing to reduce sensitivity along the entire trajectory would bring x t (t i ) closer to the upper bound √ 2T max .This is because, as seen in ( 20), a high value of x t reduces the sensitivity.

VI. VALIDATION
The proposed strategy has been validated both in a simulated environment and via real experiments.In the simulated environment, a 2 DoFs omnidirectional robot has been considered, and in the real experiments this has been translated in a UR10e pushing a mass over a table.Indeed, this task is a representative scenario in the context of automated warehouses.In fact, when the robot is unable to lift some package, e.g. because it is too heavy or there is not enough space, the package has to be pushed to the desired position.Thus, in the case of a collaborative robot, i.e. a robot that works in close contact with the human operator, the passivity of the system is necessary to ensure safety in the work cell.Therefore, the model of the system can be summarized as: where p = [m, b] is the vector of the considered uncertain parameters (the mass and the sliding friction coefficient over the floor).The chosen control action is a feedforward + friction compensation + PD force controller: where x1 = x 1 d − x 1 and x2 = x 2 d − x 2 represent the two errors in each direction, and m c and b c are the nominal values for the parameters.The control action is provided as input to the energy tank as defined in Sec.II: The energy dissipated by the system and injected in the tank is not known a priori, and by using only the nominal value b c could lead to a loss of passivity in case of an uncertain b (the tank could store more energy than what is actually dissipated).However, since the uncertainty of the friction coefficient is known, it is possible to exploit it to ensure the passivity, i.e. the energy dissipated by the system will be at All the framework has been implemented and tested on Matlab exploiting fmincon solver with an Intel Core i7-7700 and parallelizing the computation using the Matlab Parallel Computing Toolbox.Regarding the real experiment, instead, the robot is velocity controlled exploiting the Real-Time Data Exchange library integrated into the ROS Melodic Morenia meta-operating system at a frequency of 500 Hz.

A. Simulations
The overall procedure has been first validated in simulation, where it is possible to control all the parameters and to inject random but known perturbations.The nominal parameters are equal to m c = 2.0 Kg and b c = 2 kg s , while the uncertainty is δ pi = 10%p ci .The lower bound energ has instead been chosen as ε = 3.This choice has been done for two main reasons.The first one it is purely for a computational point of view.Indeed, it has been experimentally observed that lower values of ε introduce higher nonlinearities, see (18), with a consequent increment of the required optimization time.The second one is because, as shown in (20), a small value of x t increments the sensitivity of the tank and, since the main goal is to minimize x t , choosing ε not close to 0 automatically helps to reduce the sensitivity.Lastly, the initial condition and the desired final condition given to the optimization problem are listed in Tab.I.In this simulated scenario, three case studies with different cost functions have been carried out: 1) Minimize the initial energy: 1 2 x t (t i ) 2 .2) Minimize the sensitivity of the controller state at the end: 3) Minimize the sensitivity of the controller state all over the trajectory: The results of the first simulation are shown in Figures 2 -4. Figure 2 shows the evolution of the controller state and demonstrates that, for every perturbation, the tank state lies inside the tube as expected.This means that, thanks to the proposed approach, even in the worst case the energy tank does not deplete and u = u d , i.e. σ = 1.Figures 3  -4, instead, show the same results for the position and the velocity of the robot, respectively.
Focusing on the controller state, Fig. 5 shows the evolution of the controller state for the other two simulations.It is worth noting that in Fig. 5a the final tube size is smaller, while in Fig. 5b the tube is reduced all over the path.Regarding the initial tank state x t (t i ), instead, it is higher than the one in Fig. 2, since in these two cases it not minimized and, from a sensitivity point of view, it is better Fig. 3: Evolution of the system position in the perturbed cases.The red dashed lines represent the tube around the controller state, while the colored ones represent the evolution of the position for each perturbed case.Fig. 4: Evolution of the system velocity in the perturbed cases.The red dashed lines represent the tube around the controller state, while the colored ones represent the evolution of the velocity for each perturbed case.
to have x t high, see equation (20).
Lastly, a comparison in the optimization time has been performed.We generated random initial and final states and found the optimal solution for all three cost functions.For all the random comparisons, the final time t f has been set to 1.This is because, in the optimization process, it is necessary to integrate (12).Thus, different final times would affect the required optimization time.The results are summarized in Fig. 6, where it is possible to note that using 1 2 x t (t i ) 2 as cost function required more time.This is due to the fact that closely to the lower bound, the nonlinearity of σ takes an important role.This also demonstrates why it has been chosen ε = 3.

B. Experiment
The setup of the experiment is shown in Fig. 7.In order to better capture the real scenario, the friction model has been  changed from the viscous friction to the Coulumb friction: where g is the gravity acceleration.The function sign() is non differentiable in 0, but it can be approximated with a sigmoid function.This leads to: where p = [m, µ].The computation of the control action and the tank state evolution are analogous to the previous case.For the real scenario, the nominal parameters are equal to m c = 2.04 Kg and µ c = 0.15, while the uncertainties of the mass and the friction coefficient are set to δ m = 0.15 kg and δµ = 0.042, respectively.The initial condition and the desired final condition for the optimization problem are listed in Table II.
Similarly to before, two experiments have been performed: 1) Nominal experiment, m = m c , considering the tube around the tank state.2) Perturbed experiment, m ̸ = m c , considering the tube around the tank state.
For both cases, the friction coefficient µ has been experimentally estimated moving the mass at constant speed.However, this estimation is subject to uncertainties in the measurement, this is why it has been decided to not modify the friction coefficient, e.g.changing the surface.
In both the first and the second experiments, the robot is  capable of moving the mass along the planned path.Figure 8 shows that, thanks to the optimization problem, the tank state always lies inside the tube.This means that the controller is always able to implement the desired action.Figures 9 and 10, instead, demonstrate that also the evolution of the state remains confined in the tube.

VII. CONCLUSIONS AND FUTURE WORKS
In this work a new method for initializing the energy tank while being robust to uncertainties in robot/environment model parameters has been proposed.The strategy relies on the concept of closed-loop state sensitivity to build tubes that envelope all the possible deviations of the tank state w.r.t.its nominal evolution for a given range of parametric uncertainty.This machinery is used as a starting point to formulate a nonlinear optimization problem that allows to compute both the trajectory the and the initial tank energy such that the system will implement exactly the desired behaviour without depleting the tank energy for any value of the uncertain parameters.
Future works will focus on a more complex experimental validation, e.g., focusing on the uncertainties of the dynamic modeling of a robotic manipulator, or in the grasping pose.Furthermore, the overall approach may be extended and generalized for all the Port-Hamiltonian systems and not only the case of energy tanks.Lastly, it may be interesting to analyze the robustness of the approach in the presence of external inputs, e.g. an operator interacting with the robot.

Fig. 5 :
Fig. 5: Resulting evolution for random perturbations of the controller for the second and third simulations.

Fig. 7 :
Fig. 7: Setup of the experiment: the UR010e robot with a custom tool that allows pushing the mass and the ATI mini 45 F/T sensor.

Fig. 9 :
Fig. 9: Evolution of the system position in the real experiment.

Fig. 10 :
Fig. 10: Evolution of the system velocity in the real experiment.

TABLE I
Evolution of the controller state in the perturbed cases.The red dashed lines represent the tube around the controller state, while the green dotted line represents the lower bound of the tank state.The colored lines, instead, represent the evolution of the tank state for each perturbed case.

TABLE II
Fig. 8: Evolution of the controller state in the real experiment.