Mario Selvaggio
email: mario.selvaggio@unina.it

Paolo Robuffo Giordano

Fanny Ficuciello

Bruno Siciliano

Bruno Siciliano Passive

Passive Task-Prioritized Shared-Control Teleoperation with Haptic Guidance

Passive Task-Prioritized Shared-Control Teleoperation with Haptic Guidance M. Selvaggio 1 , P. Robuffo Giordano 2 , F. Ficuciello 1 and B. Siciliano 1

Abstract-Robot teleoperation is widely used for several hazardous applications. To increase teleoperator capabilities shared-control methods can be employed. In this paper, we present a passive task-prioritized shared-control method for remote telemanipulation of redundant robots. The proposed method fuses the task-prioritized control architecture with haptic guidance techniques to realize a shared-control framework for teleoperation systems. To preserve the semi-autonomous telerobotic system safety, passivity is analyzed and an energytanks passivity-based controller is developed. The proposed theoretical results are validated through experiments involving a real haptic device and a simulated slave robot.

I. INTRODUCTION

Autonomous robots are still far from being reliably and safely employed in hazardous and/or unstructured environments. Indeed, the current robots level of autonomy is still too low to have them effectively dealing with complex and nonstructured situations. A possible solution to this problem is to rely on advanced teleoperation systems that implement sharedcontrol techniques. Shared-control telerobotic systems are those in which a human operator shares the control of the slave robot with an autonomous controller to achieve a common goal. The aim of using an autonomous controller is to reduce the operator physical and mental workload when controlling a complex (e.g., highly redundant) robotic system [START_REF] Dragan | A policy-blending formalism for shared control[END_REF]. In a larger sense, shared control methods try to combine the intelligence of human operators and their sense of presence with safe and optimized task execution of autonomous control.

When highly redundant robots (e.g., mobile manipulators, dual-arm systems, humanoids) are employed at the remote site, the slave robotic system can ensure the achievement of several additional tasks (possibly having different priorities) which may be autonomously executed. In this case, the most typical choice is to rely on a task-prioritized control architecture [START_REF] Nakamura | Task-priority based redundancy control of robot manipulators[END_REF]- [START_REF] Antonelli | Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems[END_REF] and implement null-space projections [START_REF] Khatib | A unified approach for motion and force control of robot manipulators: The operational space formulation[END_REF] for the simultaneous execution of multiple hierarchical tasks. Additionally, it is fundamental for the user to receive an appropriate sensory feedback informing about the feasibility of her/his commands against the slave system constraints. To achieve this, a haptic guidance method, which informs the operator about constraints acting on the teleoperation system, needs to be designed.

For such a devised system, it is of paramount importance to guarantee a passive behaviour even during the autonomous regulation of different priority tasks. Enforcing passivity is equivalent to guarantee that the energy accumulated in the system is never larger than the power supplied through the interaction over time plus any initial stored energy. Classical passivity-based control approaches allow enforcing a passive behavior but they can result in an overly conservative control system design. To overcome this limitation, a less restrictive passivity-preserving control method is preferable.

In this work, we propose a bilateral teleoperation architecture for a task-prioritized slave robot and illustrate the possible loss of passivity that may arise when operating such a system. To the best of the authors' knowledge, there are no previous works addressing the problem of enforcing a passive behaviour of shared-control teleoperation systems involved in the realization of several prioritized tasks. We develop a passivity-based controller built upon the energytanks approach [START_REF] Franken | Bilateral telemanipulation with time delays: a two-layer approach combining passivity and transparency[END_REF] which is validated through experiments involving a real haptic device and a simulated slave robot.

II. RELATED WORKS

Related works involve two main fields: 1) Shared Control: The idea of shared control in telerobotics has appeared in various forms. For instance, it has been used to compensate for beating heart movements in robotic surgical interventions [START_REF] Rydn | Forbidden-region virtual fixtures from streaming point clouds: Remotely touching and protecting a beating heart[END_REF], to effectively avoid system constraints in dual-arm telerobotic systems [START_REF] Nicolis | Occlusionfree visual servoing for the shared autonomy teleoperation of dual-arm robots[END_REF], [START_REF] Selvaggio | Haptic-based shared-control methods for a dual-arm system[END_REF] or to combine decision making capabilities of a human operator with the high flexibility of a robot swarm [START_REF] Franchi | Shared control : Balancing autonomy and human assistance with a group of quadrotor uavs[END_REF]. More recently, the shared autonomy paradigm has started to be investigated [START_REF] Javdani | Shared autonomy via hindsight optimization for teleoperation and teaming[END_REF], [START_REF] Laghi | Shared-autonomy control for intuitive bimanual tele-manipulation[END_REF]: optimization techniques are used to infer user's goals, which are used, in turn, to provide assistance. The problem of semi-autonomous teleoperation of a redundant slave robot has been studied under communication delays in [START_REF] Liu | Semi-autonomous teleoperation in task space with redundant slave robot under communication delays[END_REF], [START_REF] Liu | Control of semi-autonomous teleoperation system with time delays[END_REF]. The authors considered a number of kinematic constraints but only numerical simulations are shown. In this work, we aim at devising a novel shared-control strategy that autonomously regulates a subset of the redundant slave robot degrees-of-freedom (DoFs) while leaving the user control over the remaining ones. This is accomplished by using the task-prioritized control approach [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF] in which the user input constitutes one of the tasks.

Haptic feedback and guidance have been shown to play a significant and promising role in shared-control applications [START_REF] Abi-Farraj | User evaluation of a haptic-enabled shared-control approach for robotic telemanipulation[END_REF], [START_REF] O'malley | Shared control in haptic systems for performance enhancement and training[END_REF]. Haptic cues can be used to increase situation awareness [START_REF] Bimbo | Teleoperation in cluttered environments using wearable haptic feedback[END_REF] and/or to effectively steer the human operator towards the safe execution of some tasks. A possible choice to generate haptic guidance is to use virtual fixtures, i.e. software generated constraints that help the operator to follow a predefined path and/or avoid forbidden regions [START_REF] Marinho | Active constraints using vector field inequalities for surgical robots[END_REF]- [START_REF] Selvaggio | Passive virtual fixtures adaptation in minimally invasive robotic surgery[END_REF]. In this work, the haptic guidance is realized through the Gradient Projection Method [START_REF] Siciliano | Kinematic control of redundant robot manipulators: A tutorial[END_REF] of opportunely defined scalar cost functions and is used to inform the user about constraints acting on the slave robotic system.

2) Passivity-based Control: Few authors have investigated stability issues caused by shared-control techniques in telerobotics. Passivity-based approaches consider the system energy flow to derive sufficient but sometimes unnecessary conditions for stability [START_REF] Ryu | Stable teleoperation with time-domain passivity control[END_REF]. For this reason, enforcing passivity can result in overly conservative parameters design, leading to degraded performances of the system.

A state-dependent approach based on the concept of energy tanks can be exploited to overcome this limitation [START_REF] Ferraguti | An energy tankbased interactive control architecture for autonomous and teleoperated robotic surgery[END_REF]. The energy-tanks passivity-based control method has been used to enforce passivity of variable impedance controllers [START_REF] Ferraguti | A tank-based approach to impedance control with variable stiffness[END_REF], [START_REF] Landi | Admittance control parameter adaptation for physical human-robot interaction[END_REF], to stably adapt to contacts in unknown environments [START_REF] Shahriari | Adapting to contacts: Energy tanks and task energy for passivitybased dynamic movement primitives[END_REF] or to switch the system topology in robot swarms [START_REF] Secchi | Bilateral teleoperation of a group of uavs with communication delays and switching topology[END_REF]. This approach is particularly suitable for teleoperation since it can activate its passifying action only when necessary, significantly improving the control performance when compared to fixed-structure passivity-enforcing controllers.

In the context of task-prioritized control, the projectors modify the energy flows in the system and destroy the precious and safety-critical property of passivity [START_REF] Dietrich | Passivation of projection-based null space compliance control via energy tanks[END_REF], [START_REF] Dietrich | Passive hierarchical impedance control via energy tanks[END_REF]. We restore this property by developing an energy-tanks-based method suitable for a task-prioritized shared-control teleoperation architecture with haptic guidance.

III. SYSTEM DESCRIPTION

A telemanipulation system, which consists of a master haptic device and a slave robotic manipulator operating in an industrial scenario, is considered. The operator is asked to carry out some tasks by teleoperating the slave robot via the haptic device. A camera is placed at the slave side to provide visual feedback to the operator as well as to recover the object/gripper relative pose. We consider a number of kinematic/workspace constraints acting on the slave side (e.g., joint limits, singularities, collisions) and provide the operator with haptic guidance to effectively avoid them.

In the following, we describe the master and slave robotic system models as well as their interconnection.

A. Master System

Let q m ∈ R m be the master manipulator generalized coordinates vector and m denote its dimension. The master is a gravity pre-compensated robotic device with the following joint space dynamics

M m (q m )q m +C m (q m , qm ) qm +B m qm = τ m +τ h , (1)
where M m (q m ) ∈ R m×m is the positive-definite and symmetric joint space inertia matrix, C m (q m , qm ) ∈ R m×m consists of the Coriolis/centrifugal terms, B m ∈ R m×m accounts for the friction term, and τ m , τ h ∈ R m are the input torques corresponding to master/slave coupling method (see Sec. III-C) and human operator interaction forces, respectively. The control forces τ m ∈ R m are exploited to provide force cues meant to inform the user about the feasibility of her/his commands against slave system constraints.

B. Slave System

The slave robot is a shared-control manipulator with q s ∈ R n being the n-dimensional vector of its generalized coordinates. The slave is modeled as an industrial manipulator with a joint velocity interface that allows to directly command qs ∈ R n . For our purposes, these velocity commands are partitioned as follows qs = qs,a + qs,u

where qs,a ∈ R n contains joint velocities corresponding to a certain number of autonomous operational space tasks, and qs,u ∈ R n represents joint velocities corresponding to operational space user inputs, respectively. Let us consider the presence of an autonomous taskprioritized controller that accomplishes the primary task while trying to fulfill a number of secondary tasks. In this case, the autonomous control input can be specified as follows

qs,a = r i=1 ΛiP i-1J † s,i σi (3)
where σi = σ * i -σ i ∈ R ri is the i-th task space error and r i denotes its dimension, such that σi = J s,i qs where J s,i ∈ R ri×n denotes the corresponding Jacobian matrix and

J † s,i ∈ R n×ri its Moore-Penrose pseudoinverse, i.e. J † s,i = J T s,i (J s,i J T s,i ) -1 , Λ i ∈ R n×n
is the diagonal and positivedefinite i-th task gain matrix, r is the number of tasks (autonomously executed) and P i ∈ R n×n the i-th null-space projector (with P 0 = I n being the n × n identity matrix). There exist several methods to compute P i : it can be obtained from the augmented Jacobian of all higher priority tasks, i.e.

Js,1,...,i =    J s,1 . . . J s,i    Pi = In -J † s,1,...,i J s,1,...,i , (4)
or by successive projections, i.e,

Pi = In -J † s,i J s,i , Pi = P1 . . . Pi-1. (5)
A more comprehensive overview of task-prioritized control methods, as well as their stability properties, is given in [START_REF] Antonelli | Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems[END_REF].

On the other hand, the user-specified control velocity qs,u is given by the master/slave coupling method and is detailed in the following section.

C. Coupling Method

The proposed master/slave bilateral interconnection considers the human operator's input as an additional task for the slave manipulator while, at the same time, haptic guidance forces are fed back to avoid system constraints. Without loss of generality, in this work we have chosen to set the operator 'task' as the one with lowest priority. Clearly, other design choices would be possible depending on the role the user input has to play in the overall task accomplishment.

The proposed coupling is realized by connecting the two systems with the following velocity-force interconnection

qs,u = PrJ † s,u ZcRcΛmJm qm, (6)
τm = -J T m ΛsR T c Z T c J † s,u T P T r ∇H (q s ) , (7)
where Λ m ∈ R 6×6 , Λ s ∈ R 6×6 are diagonal and positivedefinite interconnection gain matrices, J m ∈ R 6×m is the master manipulator Jacobian matrix, J s,u ∈ R z×n is the Jacobian matrix associated with the teleoperation task, Z c ∈ R z×6 is a selection matrix used to extract z components of the master device output velocity, R c ∈ R 6×6 is the blockdiagonal coupling rotation matrix used to transform velocity commands from the master to the slave end-effector frames, and H (q s ) : R n → R ≥ 0 is a cost function encoding the proximity to slave system constraints. Its gradient ∇H ∈ R n is used to provide haptic cues (through [START_REF] Rydn | Forbidden-region virtual fixtures from streaming point clouds: Remotely touching and protecting a beating heart[END_REF]), meant to inform the user where to move in order to effectively avoid system constraints.

Note that, when adding ( 6) to (3), the operator's commands qm are correctly considered as the lowest priority task thanks to the projector P r in [START_REF] Franken | Bilateral telemanipulation with time delays: a two-layer approach combining passivity and transparency[END_REF]. Furthermore, the coupling method given by ( 6) and ( 7) represents a power preserving interconnection if we design Λ m = Λ s , since it satisfies the relation qT m τ m + qT s,u ∇H = 0. This property will be exploited in the passivity analysis given in the next section.

IV. PASSIVITY ANALYSIS

We now proceed to analyze the system passivity when subject to the proposed control inputs. The following (standard) definition of passivity is used.

Definition 1: A system with state space model ẋ = f (x, u) ∈ R q , with initial state x (0) = x 0 ∈ R q , input vector u ∈ R l and output y = h (x, u) ∈ R l is said to be passive if there exists a positive semi-definite function S : R q → R ≥ 0, called storage function, such that

S (x (T )) -S (x0) ≤ T 0 y T (t) u (t) dt (8)
for all input signals u : [0, T ] → R l , initial states x 0 ∈ R q and T > 0. Thus, proving passivity is equivalent to finding an appropriate storage function S (x) such that

Ṡ ≤ y T u ∀(x, u). (9)
It is well known that if a system ẋ = f (x, u) is passive with storage function S (x), then the origin of the system ẋ = f (x, 0) is stable. Considering the system described by (1), ( 3), ( 6) and [START_REF] Rydn | Forbidden-region virtual fixtures from streaming point clouds: Remotely touching and protecting a beating heart[END_REF] we can use as storage function the total energy of the system V : R m × R n → R ≥ 0, i.e.

V (qm, qs) = 1 2 qT m Mm qm + H(q s ) + 1 2 r i=1 σT i σi (10)
which accounts for the master kinetic energy (first term) and for the constraint and task error 'energies' (second and third terms). The time derivative of V in ( 10) is given by (assuming

σ * i constant) V = qT m Mmqm + 1 2 qT m Ṁm qm + qT s ∇H - r i=1 σT i σi. ( 11
)
Substituting M m qm from (1) and using the usual skew symmetry of the matrix Ṁ m -2C m , we further obtain

V = -qT m Bm qm + qT m (τ m + τ h ) + qT s ∇H - r i=1 σT i σi. ( 12
)
Exploiting the operational-joint space velocity mapping σi = J s,i qs , substituting qs = qs,a + qs,u from ( 3) and ( 6) we get

V = -qT m B m qm + qT m τ m + qT m τ h + qT s,a ∇H+ + qT s,u ∇H - r k=1 σk J s,k ( qs,a + qs,u ) . ( 13
)
Using the power-preserving property of the interconnection (6-7) discussed before, and noting that r k=1 σk J s,k qs,u = 0 as qs,u contains the null-space of all higher priority tasks, we further obtain

V = -qT m Bm qm ≥0 + qT m τ h y T u + + r i=1 ΛiP i-1J † s,i σi T ∇H =w + - r k=1 σk J s,k r i=1 ΛiP i-1J † s,i σi ≥0 . (14)
Referring to [START_REF] Liu | Control of semi-autonomous teleoperation system with time delays[END_REF], the term qT m B m qm is positive-definite and represents the power dissipated by the system; qT m τ h is the input-output variables product and represents the power injected into or extracted from the system by the human operator interaction; the last term can be shown to be positive semi-definite (see [START_REF] Antonelli | Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems[END_REF]) by assuming that the tasks in (3) are orthogonal, i.e. J s,i J † s,j = O ri×ri ∀j > i ∈ {1, . . . , r} (for the successive projection method given by ( 5)), or at least linear independent, i.e. not orthogonal but such that ρ(J † s,i ) + ρ(J † s,j ) = ρ([J † s,i J † s,j ]) ∀i, j ∈ {1, . . . , r} (for the augmented inverse-based Jacobian approach given by (4)) and gains matrices are positive definite.

The second term on the rhs of ( 14) can be compactly written as

w = r i=1 wi, wi = σT i (J † ) T s,i P T i-1 Λ T i ∇H. (15)
Each w i represents the energetic coupling between the i-th autonomously regulated task and the constraints cost function H(q s ). In general, the sign of each w i cannot be determined a priori, since it depends on the effect that the regulation of the i-th task would make on H(q s ) (increase or decrease). For instance, the regulation of a task could make the slave robot moving closer to a joint limit/singularity, thereby increasing H(q s ) or viceversa. As a consequence, passivity of the system (that in our case would imply V ≤ qT m τ h ) cannot be easily guaranteed because of the arbitrary sign of the overall term w = w i , and closed-loop stability of the proposed teleoperation system could be threaten at runtime. The next section will then detail a suitable passivation strategy able to deal with this shortcoming.

V. PASSIVITY ENFORCING VIA ENERGY TANKS We employ the energy tank machinery to prevent a potential loss of passivity due to the w term in [START_REF] Liu | Control of semi-autonomous teleoperation system with time delays[END_REF]. More specifically, we add to the system dynamics an energy storing element whose aim is to recover the system dissipated energy and to use it to implement potentially 'active' actions (induced by the terms w i > 0 in ( 14)) while preserving the system passivity. In more details, the tank element has the following storage function

T (z) = 1 2 z 2 , ( 16
)
with z ∈ R being the state of the tank whose dynamics is chosen as

ż = ϕ z P d - 1 z r i=1 γiwi ( 17
)
where P d = qT m B m qm ≥ 0 represents the power dissipated by the system, w i can have any sign and represents the power extracted/injected by potential passivity violating actions, and ϕ and γ 1 , . . . , γ r ∈ [0, 1] are binary variables meant to guarantee a finite upper bound for the energy stored in the tank (see also [START_REF] Lee | Passive-set-position-modulation framework for interactive robotic systems[END_REF]). With the choice in ( 17) the time derivative of ( 16) is given by

Ṫ = z ż = ϕP d - r i=1 γiwi. (18)
The tank action on the slave side dynamics is implemented through a set of variables α 1 , . . . , α r ∈ [0, 1] used to activate or deactivate tasks based on the energy available in the tank. This mechanism is obtained by replacing (2-3) with

qs = qα s,a + qs,u qα s,a = r i=1 αiΛiP i-1J † s,i σi, (19)
that is, by weighting each term in qα s,a by the activation parameter α i . The associated power balance then becomes

V = -qT m Bm qm + qT m τ h + r i=1 αiwi - r k=1 σk J s,k qα s,a . (20)
The extended system energy function is expressed by G = V + T and its time derivative takes the form (using [START_REF] Bimbo | Teleoperation in cluttered environments using wearable haptic feedback[END_REF][START_REF] Marinho | Active constraints using vector field inequalities for surgical robots[END_REF][START_REF] Selvaggio | Enhancing bilateral teleoperation using camera-based online virtual fixtures generation[END_REF])

Ġ = V + Ṫ = -(1 -ϕ) qT m Bm qm + qT m τ h + + r i=1 (αi -γi)wi - r k=1 σk J s,k qα s,a (21)
where qα s,a is given in [START_REF] Marinho | Active constraints using vector field inequalities for surgical robots[END_REF]. By denoting with T and T the tank energy upper and lower thresholds, respectively, we define ∀i ∈ 1, . . . , r the following switching policy for ϕ,

γ i ϕ = 1 if T < T 0 otherwise γi = 0 if T ≥ T & wi < 0 αi otherwise . ( 22
)
At this point, α i can be chosen as any sufficiently smooth function 1 of the tank state T and of the input power w i that 1 Continuity of each α i w.r.t. its arguments is, indeed, needed for preserving continuity of the joint velocity command qα s,a in [START_REF] Marinho | Active constraints using vector field inequalities for surgical robots[END_REF]. satisfies α i = 0 if T <= T and w i > 0, and α i = 1 when T = T . In this work, this mechanism is implemented by choosing

αi = f T , T , T g (wi, wi) + (1 -g (wi, wi)) , (23)
where

f T , T , T = 1 2 1 -cos π T - T T - T g (wi, wi) =      0 if wi ≤ wi 1 if wi > 0 f (wi, wi, 0) otherwise , (24)
and wi < 0 is a threshold on input power. With the choice in ( 22), ( 23) and ( 24), the extended system described by ( 1), ( 6), ( 7), ( 17) and ( 19) is passive w.r.t. the input-output pair (τ h , qm ) with storage function G = V + T . It is possible to verify that (21) satisfies ( 9) in all conditions that can occur when implementing [START_REF] Selvaggio | Passive virtual fixtures adaptation in minimally invasive robotic surgery[END_REF][START_REF] Siciliano | Kinematic control of redundant robot manipulators: A tutorial[END_REF]. Indeed, the term (α i -γ i ) that multiplies w i in ( 21) is always null for w i > 0 regardless of the tank state. Furthermore, the only case in which α i = 0 and γ i = 0 occurs when w i < 0 and is used to guarantee the energy upper bound limitation. In all other cases, the choices in [START_REF] Selvaggio | Passive virtual fixtures adaptation in minimally invasive robotic surgery[END_REF] allow to implement potentially non-passive actions using the energy stored in the tank while enforcing its upper ( T ) and lower ( T ) bounds.

VI. EXPERIMENTS

A. Experimental Setup

We consider a telemanipulation system consisting of a haptic device and a 6-DoF serial manipulator in an industrial scenario endowed with a camera (Fig. 1). An operator is asked to grasp an object by teleoperating the task-prioritized sharedcontrol arm through the master system. The camera, placed at the slave side in a calibrated eye-to-hand configuration, provides visual feedback to the operator as well as the object pose in the robot base frame through standard vision-based techniques. The considered tasks are:

• autonomous regulation of y coordinate of the robot endeffector frame (F g ) to y d corresponding to the y object position (F o ) in the robot base frame (F b ) (1-DoF); • autonomous orientation control around a sphere centered in the object (2-DoFs) plus alignment of the y axis between F g and F o (1-DoF). The approach is taken from [START_REF] Abi-Farraj | A visual-based shared control architecture for remote telemanipulation[END_REF]. The user input along the remaining directions (2-DoFs) constitutes the additional lowest priority task. Our experimental setup is composed of a simulated Adept Viper 850 equipped with a Robotiq 2-finger gripper as endeffector. The master is a Force Dimension Omega 6 in which only 2 linear DoFs of the device are used to control the slave manipulator end-effector position (2-DoFs). In all the experiments, we use the successive projection method and numerically verify the successive tasks orthogonality.

Any constraint of the system can be modeled through the function H. Without loss of generality, here we consider joint limits and singularities at the slave side and provide the operator with haptic guidance to effectively avoid them. Constraints are encoded through the following functions (see [START_REF] Selvaggio | Haptic-based shared-control methods for a dual-arm system[END_REF], [START_REF] Selvaggio | Towards a self-collision aware teleoperation framework for compound robots[END_REF] for more details)

H (q s ) = Hj (q s ) + Hs (q s ) (25)
Hj (q s ) = n i=1 1 λ q + s,i -q - s,i 2 q + s,i -qs,i qs,i -q - s,i (26)
Hs (q s ) = ρe -ε det(J(qs)) (det (Js (qs))

) -β , (27)
where λ, ρ, ε and β are scalar constants, q s,i is the i-th joint coordinate, q + s,i and q - s,i are the upper and the lower limits, respectively. The corresponding gradients have been calculated symbolically using the MATLAB Symbolic Toolbox. All the parameters used in the following experiments are listed in Table I while joint limits are given in Table II.

The object position is retrieved through the blob tacker and planar homography functionalities provided by ViSP software library [START_REF] Marchand | Visp for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. The control loop run at 200 Hz. -170 -120 -120 -170 -120 -170

B. Results

First we show the system evolution when the shared control is activated without any user input (i.e., qm = 0, τ m = 0) and compare the system response with and without the passivitybased control action. The slave robot autonomously executes the two above-mentioned tasks. Figure 2 contains snapshots of the slave robot initial and final configurations from the proposed experiment. The response of the system in terms of energies, task errors, control variables and forces that would have been fed back to the user, is shown in Fig. 3 with (right column) and without (left column) the energy-tanks passivity-based control.

As it can be noticed from the upper graph, when passivity control is not activated the system undergoes an active time period (yellow shaded area) in which the total energy (given by ( 10)) increases. The task errors σi , i = 1, 2 (on the second row) are brought to zero but the forces f m = (J T m ) † τ m that the user would have experienced if the bilateral teleoperation were activated (third row) reach a peak around 3s corresponding to a maximum value of 34.02 N along the y direction. Such an intensive force can cause abrupt user's movements that may guide the slave robot to encounter severe collisions or other dangerous configurations leading, eventually, to unstable behaviours.

On the other hand, the energy-tanks passivity-based control establishes a passive behavior of the system (total energy always decreasing in the upper graph), at the expense of larger errors in the tasks regulation (second row). Indeed, the tasks executions are stopped when the energy tank gets discharged according to the control laws in ( 22) and ( 23). We then performed a bilateral teleoperation experimental test using the presented haptic shared-control architecture. In this experiment the energy-tanks passivity-based control is always active. Figure 4 shows the most significant instants of the experiment while Fig. 5 contains the time history of relevant quantities.

The experiment starts with the autonomous controller regulating the two higher priority tasks as in the previous case (Fig. 4a). Looking at the graphs in Fig. 5, autonomous actions increase the constraints cost function while the tank level drops down (analogously to the previous experiment). More specifically, this is caused by the second task driving the fifth slave robot joint towards its upper limit. The user experiences haptic guidance forces that try to keep the slave robot away from its constraints. The errors tends to zero during the subsequent time period.

The experiment continues with the user driving the slave robot toward the target object (Fig. 4b). The damping acting on the system makes the tank energy increase towards its upper bound. Around 20 s the slave robot approaches the object in a configuration close to the third joint lower limit. A similar situation happens around 32 s, when the operator moves around the object before grasping it. In this cases, the cost function increases but the first two tasks have reached steady-state, thus the system is passive with respect to the user actions. This leaves the tank energy level unchanged. Haptic guidance forces help the operator to correct the slave robot configuration around the grasping point.

To further demonstrate the capabilities of the proposed method, we move the target object along the y direction by activating the conveyor belt around 38 s (Fig. 4c). Correspondingly, we register an increase in the second task error norm (red shaded area in Fig. 5). The tank energy level drops down again and the second task execution is slowed down. The user starts again teleoperating and, following the haptic guidance forces, drives the robot towards the new object position. The energy tank get replenished by both the dissipated power and passive actions while the second task error approaches zero around 41 s. The grasping task is successfully accomplished around 63 s as shown in Fig. 4d.

Both experiments are also shown in the video accompanying this paper.

VII. CONCLUSION

In this paper we introduced the concept of task-prioritized shared-control for remote teleoperation of redundant robots. The proposed method is endowed with haptic guidance used to steer the slave manipulator away from its kinematic constraints. We designed a power-preserving coupling, provided the teleoperation system passivity analysis and propose a passification strategy using energy-tanks passivitybased control. The passivity control action dynamically stops the tasks that drive the slave robot in dangerous workspace configurations, thus preventing the user from feeling large/abrupt haptic forces. The system is evaluated in remote manipulation experiments performed with a real haptic device and a simulated slave robot. Results show that the methodology effectively lets the operator accomplishing the task while ensuring safety of the telerobotic systems.

As future works we aim to endow the method with task priority switching capabilities that can make the teleoperation system even more flexible and performing.

Fig. 1 .

 1 Fig.1. Experimental setup for task-prioritized shared-control teleoperation of a 6-DoF manipulator: z and z d denote the current and desired end-effector frame vectors for the shared-control object grasping taken from[START_REF] Abi-Farraj | A visual-based shared control architecture for remote telemanipulation[END_REF].

Fig. 2 .

 2 Fig. 2. Autonomous task regulation experiment. Initial system configuration (a) and final robot configurations without (b) and with the energy-tanks passivity-based control (c).

Fig. 3 .

 3 Fig. 3. Comparison of energies, error norms, forces and control variables during the autonomous tasks regulation without (left) and with energy-tanks (right). The yellow shaded area indicates the active time period.

Fig. 4 .

 4 Fig. 4. Shared-control teleoperation experiment. (a) Initial system configuration; (b) robot approaching the object; (c) object moving; (d) robot grasping the object.

Fig. 5 .

 5 Fig.5. Time evolution of system energies, error norms, haptic guidance forces, relevant control quantities and joint velocities during the sharedcontrol teleoperation experiment. The red shaded area indicates the time slot in which the object is moved.

This project was partially supported by the IEEE RAS Techincal Committee on Haptics under the "Innovation in haptics" research programme and by the programme STAR, financially supported by UNINA and Compagnia di San Paolo.