Nicolò Pedemonte
email: nicolo.pedemonte@irisa.fr

Firas Abi-Farraj
email: firas.abi-farraj@irisa.fr

Paolo Robuffo Giordano

Visual-Based Shared Control for Remote Telemanipulation with Integral Haptic Feedback

 we have recently proposed a visual-based shared control architecture meant to facilitate a human operator in controlling two remote robotic arms (one equipped with a gripper and another with a camera) during remote manipulation tasks of nuclear waste via a master device. The operator could then receive force cues informative of the feasibility of her/his motion commands during the task execution. The strategy presented in [1], albeit effective, suffers however from a locality issue since the operator can only provide instantaneous velocity commands (in a suitable task space), and receive instantaneous force feedback cues. On the other hand, the ability to 'steer' a whole future trajectory in task space, and to receive a corresponding integral force feedback along the whole planned trajectory (because of any constraint of the considered system), could significantly enhance the operator's performance, especially when dealing with complex manipulation tasks. The aim of this work is to then extend [1] towards a planning-based shared control architecture able to take into account the mentioned requirements. A human/hardware-in-the-loop experiment with simulated slave robots and a real master device is reported for demonstrating the feasibility and effectiveness of the proposed approach.

I. INTRODUCTION

When operating in hazardous environments such as nuclear sites [START_REF] Schilling | Telerobots in the nuclear industry: a manufacturer's view[END_REF], outer space [START_REF] Wright | Driving a rover on mars using the rover sequencing and visualization program[END_REF] or underwater [START_REF] Murphy | Use of remotely operated marine vehicles at minamisanriku and rikuzentakata japan for disaster recovery[END_REF], robotic telemanipulation becomes essential to guarantee the operator's safety. The panoply of assisted teleoperation systems proposed in the literature includes classical techniques such as virtual fixture [START_REF] Rosenberg | Virtual fixtures: Perceptual tools for telerobotic manipulation[END_REF], [START_REF] Abbott | Haptic virtual fixtures for robot-assisted manipulation[END_REF] or shared control frameworks that merge robot autonomy and human supervisory capabilities [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF], [START_REF] Masone | Interactive Planning of Persistent Trajectories for Human-Assisted Navigation of Mobile Robots[END_REF]- [START_REF] Masone | Semi-autonomous Trajectory Generation for Mobile Robots with Integral Haptic Shared Control[END_REF]. In a typical shared control architecture, the human operator is in charge of the high-level decision making and all low-level operations are autonomously controlled. Nevertheless, the task allocation between the human operator and the robotic system remains one of the main challenge of assisted teleoperation [START_REF] Inagaki | Adaptive automation: Sharing and trading of control[END_REF].

In [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF], a shared control framework for teleoperating a system on a nuclear site was presented. The work was motivated by the European H2020 "Robotic Manipulation for Nuclear Sort and Segregation" (RoMaNS) project 1 . In the RoMaNS scenario, a human operator has access to a system consisting of two robotic arms, one equipped with a gripper Fig. 1: Left figure: containers with unknown (or partially known) contents and mixed contamination levels that need to be examined, sorted and separated. Right figure: new special storage containers where contaminated waste must be placed. and the other one with a camera, with the goal of approaching and grasping nuclear waste for sort and segregation purposes (see Fig. 1). The shared control architecture presented in [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF] consisted in an instantaneous motion control of the gripper manipulator for approaching the target object, and of corresponding instantaneous force cues informing the operator about the feasibility of her/his (again instantaneous) motion commands. Due to the complexity of its motion, the camera was instead autonomously controlled. The main limitation of the architecture presented in [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF] lies in its 'local' nature, which does not provide the operator with the possibility of modifying the future behavior of the robot motion, nor of receiving force cues informative about the future consequences of her/his actions. On the other hand, the possibility of 'affecting the future' (i.e., over some future time window) and to receive a corresponding sensible force feedback would be of great importance for facilitating the operator's task, especially in complex manipulation environments.

In order to cope with this issue, in this paper we consider the same scenario of [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF] and extend it by including the possibility of acting on a whole future trajectory for the gripper. In particular, rather than controlling some DOFs of the gripper manipulator arm and obtaining an instantaneous force feedback w.r.t. any pre-defined constraint of the system (in [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF] joint limits were considered), the human operator is now given the possibility of modifying online the trajectory for approaching the object of interest with the aid of predictive force feedback cues informing about any constraint of interest. The force feedback is denoted as predictive since it informs the operator about the feasibility of her/his planned (future) trajectory against the system constraints, and thus is evaluated on a planned motion yet to be executed. The concept of predictive haptic feedback was introduced in [START_REF] Masone | Semi-autonomous Trajectory Generation for Mobile Robots with Integral Haptic Shared Control[END_REF], where it was applied to the problem of shaping the 2D trajectory of a mobile robot via an input device. In this work, the ideas of [START_REF] Masone | Semi-autonomous Trajectory Generation for Mobile Robots with Integral Haptic Shared Control[END_REF] are instead extended to the case of a human operator in partial control of the pose of a 6-dof manipulator with the goal of approaching an object of interest to be later manipulated.

The rest of the paper is organized as follows. In Sect. II the general problem is introduced, while the shared control architecture is described in detail in Sect. III. Section IV reports some experimental results and Section V concludes the paper and discusses some future directions.

II. PROBLEM SETTING

The scenario considered in this paper consists of two 6-dof serial manipulators, one equipped with a monocular (calibrated) camera and the other one with a gripper, aiming at grasping an object of interest (see Fig. 2). We consider three frames of reference: a frame

F O : {O O ; X O , Y O , Z O } attached to the object to be grasped, a frame F G : {O G ; X G , Y G , Z G }
attached to the gripper, and a frame F C : {O C ; X C , Y C , Z C } attached to the camera. We assume that Z G is aligned with the gripper approaching direction, and that (as usual) Z C is aligned with the camera optical axis.

We let

( C P G , C R G ) ∈ R 3 × SO(3) represent the 3D pose of F G w.r.t. F C expressed in F C and, similarly, ( C P O , C R O ) ∈ R 3 × SO(3) represent the 3D pose of F O w.r.t. F C expressed in F C .
In the context of the RoMaNS project, we can assume that an accurate enough 3D model of both the object to be grasped and of the gripper is available beforehand. This allows to leverage any model-based tracker, such as those present in the ViSP library [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF], for retrieving online a reliable estimation of the camera/object and camera/gripper relative poses in the camera frame. Fig. 2: An illustrative representation of the two 6-dof serial manipulator arms equipped with a camera and a gripper, respectively, together with other quantities of interest.

The goal of the proposed shared control architecture is to (i) let a human operator modify online the approaching trajectory towards the object to be grasped via a forcefeedback device, (ii) let an autonomous algorithm verify that the commanded (desired) trajectory respects all the possible constraints of the robotic system and, in case it does not, modify it accordingly, (iii) provide the human operator with online force cues informing about any discrepancy between the commanded trajectory and the actual one (modified by the autonomous algorithm in order to cope with the robot constraints), thus informing about the future consequences of the operator's actions and, finally, (iv) let an autonomous algorithm control the camera motion so as to keep a suitable vantage point w.r.t. the observed scene (i.e., both the gripper and the object). We now proceed to detail the components of the shared control architecture. Figure 3 illustrates the proposed framework. Given an initial trajectory for the gripper manipulator, the human operator is allowed to modify the trajectory via the input device while the autonomous corrector verifies that all operator's modifications respect the constraints of the system. The haptic feedback is designed in order to assist the user during the task execution, mantain the trajectory away from dangerous configurations and keep the user aware of any autonomously imposed adjustments of the trajectory. The trajectory planning problem for a 6-dof serial manipulator is split into the position and the orientation components. We will then consider two trajectories, γ(t) for the gripper position and η(t) for the gripper orientation. Both the human operator and the autonomous correcting algorithm will be allowed to modify γ and η at runtime. Figure 3 is detailed in this section. In III-A the trajectory planning problem is introduced and the block "Trajectory updates" explained. In III-B, the "Human Guidance" component is presented. Then, section III-C focuses on the autonomous correcting algorithm while section III-D details the haptic feedback. Finally, in III-E, the control of the camera is reviewed.

III. SHARED CONTROL ARCHITECTURE

A. Trajectory planning

The trajectory planning algorithm is performed on the gripper position and orientation

( C P G , C R G ) w.r.t. the target object ( C P O , C R O ). Both ( C P G , C R G ) and ( C P O , C R O )
are measured by the camera in order to provide the system with an accurate estimation of the gripper and object poses.

Consider the position problem. We choose to represent the trajectory γ as a cubic B-spline. Cubic B-splines are piecewise curves which guarantee the continuity of velocities and accelerations and are a typical choice for representing trajectories [START_REF] Masone | Semi-autonomous Trajectory Generation for Mobile Robots with Integral Haptic Shared Control[END_REF], [START_REF] Biagiotti | Trajectory Planning for Automatic Machines and Robots[END_REF]. A cubic B-spline can be completely parametrized by a sequence of scalars U (knot vector) and a vector of points x = [x 0 , ..., x n ] (control points) with x i ∈ R 3 and n ∈ N. The position trajectory will then be defined by:

γ(x, u) = n i=0 x i B i (u), (1)
where u ∈ [0, 1] is the independent variable representing the position along the curve (u = 0 being the starting point and u = 1 the final point) and B i are the basis functions [START_REF] Biagiotti | Trajectory Planning for Automatic Machines and Robots[END_REF], [START_REF] Piegl | The NURBS book[END_REF]. Note that, given the knot vector, a finite number n + 1 of control points is sufficient to define the whole trajectory. This property of B-splines is crucial for our application since it reduces an infinite-dimensional problem (optimization of a continuous trajectory) to a finitedimensional one (optimization of the n + 1 control points).

For the orientation problem, we exploit [START_REF] Kim | A general construction scheme for unit quaternion curves with simple high order derivatives[END_REF] where an analytical expression for a cubic B-spline quaternion curve in S 3 was presented. Consider the same knot vector U and a vector of control points (quaternions) p = [p 0 , ..., p n ], with p i ∈ S 3 . The whole orientation trajectory is defined by:

η(p, u) = p B0(u) 0 n i=1 exp(ω i B i (u)), (2)
where B are cumulative basis functions [START_REF] Kim | A general construction scheme for unit quaternion curves with simple high order derivatives[END_REF], ω i = log(p -1 i-1 p i ) and log(•) and exp(•) represent respectively the logarithmic map, log : S 3 → R 3 , and the exponential map, exp : R 3 → S 3 (see Appendix A and [START_REF]A compact differential formula for the first derivative of a unit quaternion curve[END_REF]). The cubic B-spline quaternion curve is shown to respect some important B-spline properties such as C 1 -continuity and local controllability [START_REF] Kim | A general construction scheme for unit quaternion curves with simple high order derivatives[END_REF].

As shown in Fig. 3, the human operator and the autonomous corrector can modify the trajectories γ and η by commanding the linear and angular velocities of the control points of the two trajectories. Hence, the human operator will command ( ẋH , ω H ) and the autonomous corrector will command ( ẋA , ω A ). The human and the autonomous contributions are then merged as follows (corresponding to the block "Trajectory updates" in Fig. 3). Concerning the control points of the position trajectory, the following merging rule is implemented:

x(t) = x(t 0 ) + t t0 ẋH (τ )dτ + t t0 ẋA (τ )dτ, (3)
where ẋH represents the velocity of the control points of γ commanded by the human operator, and ẋA corresponds to the velocity of the control points of γ commanded by the autonomous correction. The equivalent of (3) for the control points of the orientation trajectory is:

p(t) = p(t 0 ) • p H (t) • p A (t) (4)
where p H corresponds to the rotation commanded by the human operator, p A represents the orientation computed by the autonomous corrector and • is the usual quaternion multiplication rule. The quantities p H (t) and p A (t) can be obtained from the quaternion integration rule as follows:

p H (t) = exp( 1 2 t t0 ω H (τ )dτ ) p A (t) = exp( 1 2 t t0 ω A (τ )dτ ) , (5)
where ω H represents the angular velocity applied by the operator and ω A corresponds to the angular velocity imposed by the corrector.

B. Human Input

In this section, the block "Human Guidance" of Fig. 3 will be detailed. As we stated above, the human operator is given the opportunity of commanding the quantities ẋH and ω H in order to modify the trajectories γ and η to his/her convenience. Moreover, we also provide the operator with he possibility of moving along the current trajectories backwards and forwards at will. The operator will then generate the commands [λ, λ u ]

T ∈ R 4 by controlling the position of the input device along four independent DOFs. The command λ u permits to the operator to move the gripper manipulator along the planned trajectories when all the autonomous corrections are completed. On the other hand, the command vector λ ∈ R 3 is mapped onto the control point space R 7 in order to modify both trajectories. We now proceed to detail this mapping. We consider in this work the approaching (pre-grasping) phase. We assume that, during this specific phase, the operator may need to move the gripper around the target object in order to choose the best pose to grasp. Let the two initial trajectories γ 0 (x, u) and η 0 (p, u) be defined such that the gripper is, in its final pose, both aligned w.r.t. the target object and at a certain distance r from it, and let Q : R 3 → R 7•(n+1) be the nonlinear mapping matrix from the input device space to the control point space, where Q can be decomposed as

Q = [Q 1 . . . Q i . . . Q n ]
T . Then, the velocity of the control points x i and p i commanded by the human operator are defined as:

ẋH,i = k H,i Q i • λ 0 , ω H,i = k H,i Q i • 0 λ , (6)
where

k H,i ∈ K H = [0, k H,1 , ..k H,n-1 , 1] is the vector of gains. Matrix Q i ∈ R 7×3
maps the input commands λ into the velocities of the i-th control points of γ(x, u) and η(p, u). In our framework, following [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF], we define

Q i = [m 1,i , m 2,i , m 3,i ]
where m 1,i , m 2,i and m 3,i are the three motion directions defined as follows: let s i be the 3D direction towards the object to be grasped for each control point x i defined as s i = P s,i P s,i , where P s,i = O O -x i expressed in the object frame F O . We then take

m 1,i =   -[s i ] × e y - G R O,i P s,i e y P s,i   , m 2,i =   [s i ] × e x G R O,i P s,i e x P s,i   , m 3,i = 0 G R O,i s i , (7)
with e x = [1 0 0] T , e y = [0 1 0] T . As explained in [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF], these three motion directions allow to control the gripper distance and alignment w.r.t. the target object with an intuitive physical meaning. In particular, m 1,i and m 2,i correspond to two coordinated motions (linear/angular velocity) that displace the gripper over a sphere centered at the target object and m 3,i realizes a rotation about s i , (see 4).

Fig. 4: Visualization of the 3 motion directions actuated by the human operator when commanding the input vector λ.

The mapping ( 6) applies to all control points x i and p i such that i ≥ j k with j k ∈ N ≤ n. The index j k can be readily found by implementing the classical algorithm for determining the knot span [j k-1 , j k ) corresponding to the current value of the independent variable u [13], [START_REF] Piegl | The NURBS book[END_REF]. Given a certain value of u, for i ≥ j k , modifying the control points x i and p i will not change the current pose of the gripper manipulator. Hence, only the part of the two trajectories yet to be covered by the gripper/manipulator can be modified by the operator. Note that the control points x 0 and p 0 never move (being k H,0 in (6) equal to 0).

C. Autonomous Correction

In this section, the block "Autonomous Corrector" of Fig. 3 will be detailed. In our framework, the goal of the autonomous algorithm is to ensure that the current trajectories respect any predefined constraint, e.g., joint limits, singularities, or collision avoidance. Let H(q) be the cost function encoding such constraint(s) and q ∈ R 6 the joint vector of the manipulator carrying the gripper. The objective of the autonomous corrector is to modify the vectors of control points x and p in order to minimize the cost function H(q) along the whole trajectories γ and η. For a given u, the trajectories γ(x, u) and η(p, u) correspond to a specific robot configuration q(u) via the robot inverse kinematics. Letting J P (q) and J O (q) be the geometric Jacobian matrices related to the position and the orientation of the gripper, the following then holds

   ∂γ(x, u) ∂t ∂η(p, u) ∂t    =     ∂γ(x, u) ∂q(u) ∂η(p, u) ∂q(u)     ∂q(u) ∂t = = J P (q(u)) M QP (p) • J O (q(u)) q(u) (8)
where M QP (p) is obtained from the quaternion propagation rule (see Appendix B). For each position u along the trajectories γ(x, u) and η(p, u), one also has H(q) = H(q(u)) = H(γ(x, u), η(p, u)). In order to minimize the cost function H(γ(x, u), η(p, u)) along the whole trajectories, the autonomous correction ẋA and ω A are then defined as the negative partial derivative of the cost function w.r.t. γ(x, u) and η(p, u), respectively, see also [START_REF] Masone | Semi-autonomous Trajectory Generation for Mobile Robots with Integral Haptic Shared Control[END_REF].

Consider the position problem. The trajectory γ will be autonomously modified according to the following update rule

∂γ(x(t), u) ∂t = - γ ∂H(γ(x, u), η(p, u)) ∂γ(x, u) T du. ( 9
)
The velocity ∂γ(x(t), u)/∂t is mapped onto the R 3(n+1) space of control points by using the pseudo-inverse (∂γ(x, u)/∂x) † in order to invert the relation

∂γ(x, u) ∂t = ∂γ(x, u) ∂x ẋ.
Here u = 0 because we assume that the operator does not move the robot along the trajectories while the autonomous correction is in progress. The autonomous correction in terms of control points can then be expressed by:

ẋA = - γ ∂γ(x, u) ∂x † ∂H(q(u)) ∂q(u) ∂γ(x, u) ∂q(u) † T du, (10)
where ∂γ(x, u)/∂q(u) = J P (q(u)) from ( 8). The term (∂γ(x, u)/∂x)

† can be easily computed thanks to the basic B-spline properties (see Appendix B). The computation of ∂H(q(u))/∂q(u) is straightforward and depends on the particular choice of function H(q). In the same spirit, the trajectory η for the orientation problem are autonomously modified as follows:

∂η(x(t), u) ∂t = - η ∂H(γ(x, u), η(p, u)) ∂η(p, u) T du, (11)
which leads to:

ṗA = - η ∂η(p, u) ∂p † ∂H(q(u)) ∂q(u) ∂η(p, u) ∂q(u) † T du, (12)
where ∂η(p, u)/∂q(u) = M QP (p) • J O (q(u)) from ( 8). The term ∂η(p, u)/∂p is described explicitely in Appendix B. Vector ω A can be easily computed from [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] as ω A = log( ṗA ). The autonomous correction ẋA and ω A are then merged with the human operator's modifications (6) as described in (3) and (4).

D. Haptic Feedback

The haptic feedback is designed in order to assist the operator during the task execution and inform him/her about how well the system is following the commanded motion λ. Specifically, the operator must be aware if any modification is being applied to the trajectories.

Following the classical bilateral force-feedback framework [START_REF] Hokayem | Bilateral teleoperation: An historical survey[END_REF], [START_REF] Nuño | Passivity-based control for bilateral teleoperation: A tutorial[END_REF], we then assume the presence of a master device upon which the operator can act for sending the commands λ to the slave side (the gripper/manipulator arm) and receiving force feedback cues. The master device is modeled as a generic (gravity pre-compensated) mechanical system

M (y M )ÿ M + C(y M , ẏM ) ẏM = τ + τ h ( 13
)
where y M ∈ R m is the device configuration vector (with same dimension as the human commands λ), M (y M ) ∈ R m×m is the positive-definite and symmetric inertia matrix, C(y M , ẏM ) ∈ R m×m accounts for Coriolis/centrifugal terms, and τ , τ h ∈ R m are the control and human forces, respectively.

The human control actions are implemented by setting

λ = K λ y M , (14)
with K λ ∈ R m×m being a diagonal matrix of positive scaling factors. This coupling then allows the operator to directly affect the control points of the planned trajectories along the motion directions m i by adjusting the position of the master device (Fig. 5). The force feedback is instead designed as

τ = -B M ẏM -K M y M + f . (15)
Here, B M ∈ R m×m is a positive definite damping matrix Fig. 5: Representation of λ on the haptic device.

for stabilizing the haptic device and K M ∈ R m×m is a positive definite diagonal matrix meant to implement a 'soft spring' centered at the device rest position2 . Vector f = [. . . f i . . .] T ∈ R m represents instead the force cues provided to the human operator: as explained, the design of these cues is aimed at informing the operator about the feasibility of each trajectory update w.r.t. possible constraints/limitations of the gripper/arm system such as, for instance, proximity to joint limits, to singularities, self-collisions or to collisions with the surrounding environment. We now proceed to detail the cueing algorithm which will be then exploited in Sect. IV. The haptic feedback must inform the operator about how well the actual trajectories γ(x, u) and η(p, u) are following his/her commands λ despite the possible modifications of the autonomous corrector component. In order to obtain this goal, we map the autonomous corrector's commanded velocities ẋA and ω A onto the human input command space using the pseudo-inverse of the mapping matrix

Q introduced in III-B. Be Π = [Π 1 . . . Π i . . . Π n ]
T ∈ R 7(n+1)×1 , with

Π i = [ ẋA,i , ω A,i ] T ∈ R 7×1 .
The haptic cues f are then defined as:

f = -K τ Q † Π, ( 16
)
where K τ is a constant gain matrix. The forces f will push the operator away from any dangerous configuration with an intensity proportional to H(q) along the directions of the trajectory modifications imposed by the autonomous corrector. In conclusion, the haptic cues will provide the operator with a high-level information about the feasibility of his/her commanded trajectories w.r.t. the system constraints along a future time window (the time extension of the trajectory itself), rather than by providing instantaneous cues as previously proposed in [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF].

As a final step, we make use of the passive set-position modulation (PSPM) algorithm [START_REF] Lee | Passive-set-position-modulation framework for interactive robotic systems[END_REF] for coping with the typical stability issues of any bilateral force feedback loop because of communication delays, packet losses, master/slave kinematic/dynamic dissimilarities, and other shortcomings. To this end, let ψ(t) = K -1 M f (t) and rearrange ( 15) as

τ = -B M ẏM -K M (y M -ψ(t)). ( 17
)
The PSPM action modulates the (arbitrary) signal ψ(t) into a possibly attenuated version ψ(t) which, when plugged into ( 17), ensures input/output stability (passivity) of the master device. This is then sufficient for guaranteeing stability (passivity) of the overall bilateral teleoperation, see [START_REF] Lee | Passive-set-position-modulation framework for interactive robotic systems[END_REF] for more details and [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF], [START_REF] Lee | Semi-Autonomous Haptic Teleoperation Control Architecture of Multiple Unmanned Aerial Vehicles[END_REF] for some recent examples of the use of the PSPM algorithm.

E. Camera Control

The goal of the vision system goal is to retrieve the object and gripper poses required by the shared control framework presented above. The camera manipulator is then controlled using an IBVS approach so as to keep a suitable vantage point w.r.t. the observed scene, i.e., both the gripper and the object. All the details on this component of the architecture can be found in [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF].

IV. EXPERIMENTAL RESULTS

In this section we report the results of a set of experiments designed to illustrate and validate the shared control architecture presented above and to prove the effectiveness of the force cues. The master side consists of the Haption VIRTUOSE 6D haptic device 3 , a high performance force feedback device with three translational DOFs and three rotational DOFs. The maximum force/torque is about 30 [N]/3 [Nm], the workspace has a spherical-like shape with an approximated radius of 0.9 [m], and the device exchanges data over ethernet with a control PC at 1 kHz. Four DOFs of the Haption device were left unconstrained. Three of them are needed for actuating the m = 3 motion directions m i detailed in [START_REF] Masone | Interactive Planning of Persistent Trajectories for Human-Assisted Navigation of Mobile Robots[END_REF]. As explained in Sect. III-D, the position of the master along these three DOFs was coupled to the input commands λ via [START_REF] Piegl | The NURBS book[END_REF]. The fourth DOF is used by the operator to move the gripper along the trajectory backwards and forwards. The remaining two DOFs were constrained via software to a constant value. The slave side consists of two 6-DOF Viper S850 robotic arms carrying the gripper and the camera, and simulated in the popular V-REP environment 4 . The poses of the gripper and of the target object in the camera frame were reconstructed by feeding the model-based ViSP tracker [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] with the segmented location of some fiducial markers acquired at 30 Hz, see Fig. 6. Fig. 6: The two simulated 6-DOF manipulators carrying the camera and the gripper and, in the small window on the bottom-right, the camera image. The yellow sphere represents the final pose of the gripper, whereas the cyan line represents the initial trajectory.

We considered in this work proximity to joints limits and to singularities as representative constraints of the gripper/manipulator arm system. The function H(q) in ( 9) and ( 11) is then expressed as

H(q) = H JL (q) + H S (q), (18)
where H JL (q) represents the cost function for the joint limit constraint and H S (q) the cost function for the singularity constraint. The joint limit cost function is defined as a classical quadratic penalty cost

H JL (q) = k JL 6 i=1 h i (q), with h i (q) =     
(q i -(q i,max -q th )) 2 , if q i ≥ q i,max -q th (q i,min + q th -q i ) 2 , if q i ≤ q i,min + q th 0, otherwise , where (q i,max , q i,min ) are the maximum/minimum range for the i-th joint and q th is a user-defined threshold defining the activation region inside which the user will receive a force feedback. The cost function related to the singularity 4 www.coppeliarobotics.com.

constraint is instead defined as the inverse of the determinant of the geometric Jacobian matrix J :

H S (q) = k S det(J) , if det(J ) ≤ 0, otherwise ,
where is a user-defined constant parameter, and k S and k JL are scaling factors. We now report two different experiments conducted for validating the described shared control architecture. The reader is also encouraged to watch the video attachment for a better understanding of the results of the experimental session.

A. First Experiment

The first experiment is designed in order to show the main features of our approach, i.e., (i) the possibility of modifying the current trajectories (γ and η) by actuating the m motion directions, (ii) the possibility of moving along the current trajectories, and (iii) the assistance provided by the haptic feedback when the trajectories are in proximity of joint limits and singularities. The experiment can be split into three phases where, in each of them, a single motion command is activated. The trajectories are modified until they reach a joint limit/singular configuration. In the first phase (7 [s] ≤ t ≤ 34 [s]), the operator sends the motion command λ 2 . In the second phase (51 [s] ≤ t ≤ 57 [s]), λ 1 is activated. Finally, in the third phase (68 [s] ≤ t ≤ 88 [s]), the operator commands λ 3 . Between the first and the second phase (43 [s] ≤ t ≤ 47 [s]) and between the second and the third phase (64 [s] ≤ t ≤ 69 [s]) the operator commands the manipulator to move forward along the trajectory.

Figures 7(a-c) report the experimental results. In particular, the behavior of the haptic cues f (Fig. 7b), the value of the cost function H(q) (Fig. 7c), and the user commands λ and λ A (Fig. 7a) are shown over time. The three phases show the system reaction to the three possible motion commands. It is worth noting that in the first two phases the highest force cue is produced along the direction of the commanded motion. In the third phase, on the other hand, the user activates the input λ 3 but the haptic feedback is mainly projected along the motion direction m 1 (see Fig. 7b). This behavior might not seem intuitive but it can be explained by the gradient-based algorithm: in this situation, the fastest way to reduce the cost function was to move along the direction m 1 rather than pushing back the user along m 3 . Finally, it is interesting to observe how the haptic cues in Fig. 7b are activated when the cost function H(q) ≥ 0 (Fig. 7c). The algorithm will autonomously correct the trajectories γ and η until H(q) is minimized.

B. Second Experiment

The second experiment is meant to assess the effectiveness of the haptic feedback during a possible task execution. In this case, the human operator commands a random trajectory modification for purposely reaching joint limits and experiencing the corresponding force feedback. Figures 8(a-c) report the experimental results. As it can be observed in (c) behavior of the scalar cost functions H(q) JL for joint limits and H(q) S for singularities, quantifying the proximity to the relative constraint.

Fig. 8a, the operator initially sends the motion commands

λ 1 (4 [s] ≤ t ≤ 33 [s]) and λ 2 (7 [s] ≤ t ≤ 33 [s]
) and successively also λ 3 (17 [s] ≤ t ≤ 33 [s]). At t 33 [s], a joint limit is reached and H(q) rapidly increases (see Fig. 8c). The haptic cues (Fig. 8b) are consequently activated and the operator is forced to move backwards along λ. This backward motion is particularly evident along λ 1 , where the haptic feedback is the highest. The operator tries one more time to move forward along λ 1 but she/he is repulsed again by the haptic feedback (at t 43 [s]). Eventually, the operator commands the robot to move along the trajectory and the gripper reaches the final pose (55 [s] ≤ t ≤ 73 [s]).

In conclusion, the designed force cues proved to be both informative and efficient while assisting the user in "steering" the trajectories away from undesired configurations by either moving back along the operator's commanded direction, or by manoeuvring over the other available motion directions as a function of the magnitude (and sign) of the received haptic information.

V. CONCLUSIONS

This paper extended the work presented in [START_REF] Abi-Farraj | A visualbased shared control architecture for remote telemanipulation[END_REF] to a planning-based shared control architecture. In the proposed framework, the human operator is given the possibility of modifying two initial trajectories (γ 0 for the position of the gripper/manipulator and η 0 for its orientation) via a haptic device. In parallel, an autonomous corrector verifies that all human modifications respect the system constraints, such as (in the reported cases) joint limits and singularities. When the human-modified trajectories are in proximity of any of JL for joint limits and H(q) S for singularities, quantifying the proximity to the relative constraint.

these constraints, the autonomous system is able to steer the trajectories away from the constraints by minimizing the associated cost function. The autonomous intervention is then converted into a set of force cues fed to the human operator in order to keep him/her aware of the feasibility of his/her commanded motions, and informed about any change autonomously imposed to the current trajectories. The effectiveness of our proposed shared control architecture with integral haptic cues was then demonstrated in a set of experiments presented in the last section of the paper. In the next future, two real 6-dof manipulators equipped with a gripper/camera will be employed to validate the planning-based shared control architecture. To this end, we will use vision-based reconstruction methods able to handle more complex scenes than the ones considered in this work. Furthermore, we are interested in evaluating other criteria for generating force cues other than the system intrinsic constraints, such as, for instance manipulability or collision avoidance measures. Finally, we also planning to run a set of user studies in order to assess the effectiveness of the described algorithm in a principled way.

ρ = p -1 j-1 p j . Consider v = [v 1 , v 2 , v 3 ] T ∈ R 3 , from the definition of exp(v), it follows that ∂exp(v) ∂v =        -s v v 1 -s v v 2 -s v v 3 c v 2 -s v 3 v 2 1 + s v c v 2 -s v 3 v 1 v 2 c v 2 -s v 3 v 1 v 3 c v 2 -s v 3 v 1 v 2 c v 2 -s v 3 v 2 2 + s v c v 2 -s v 3 v 2 v 3 c v 2 -s v 3 v 1 v 3 c v 2 -s v 3 v 2 v 3 c v 2 -s v 3 v 2 3 + s v       
where c = cos v and s = sin v (see also [START_REF]A compact differential formula for the first derivative of a unit quaternion curve[END_REF]). Finally, from [START_REF] Xu | The theory of quaternion matrix derivatives[END_REF],

d 1 = ∂(p -1 i-1 p i ) ∂p i = [1, 0] T , d 2 = ∂(p -1 i p i+1 ) ∂p i = -p -1 i • R{p i }.
Consider the quaternion p = w v . From the quaternion propagation rule: where ω = J O q is the angular velocity [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF].

ṗ = ẇ v = -1 2 v T

Fig. 3 :

 3 Fig. 3: An illustration of the proposed shared control framework.

Fig. 7 :

 7 Fig.7: Results of the first experiment.(a) Behavior of the operator's commands λ, λ A for actuating the motion directions m i and moving along the trajectories. (b) Behavior of the three force cues f i displayed to the human operator during the robot motion; (c) behavior of the scalar cost functions H(q) JL for joint limits and H(q) S for singularities, quantifying the proximity to the relative constraint.

Fig. 8 :

 8 Fig.8: Results of the second experiment. (a) Behavior of the operator's commands λ, λ A for actuating the motion directions m i and moving along the trajectories. (b) Behavior of the three force cues f i displayed to the human operator during the robot motion; (c) behavior of the scalar cost functions H(q) JL for joint limits and H(q) S for singularities, quantifying the proximity to the relative constraint.

  APPENDIX AConsider p = [w, v] T ∈ S 3 . The logarithmic map log :S 3 → R 3 is defined by log(p) = (arccos w• v ||v|| ) ∈ R 3 [16]. Consider v ∈ R 3 . The exponential map exp : R 3 → S 3 is defined by exp(v) = cos ||v||, sin||v|| ||v|| • v T if v = 0, else exp(v) = [1, 0] T[START_REF]A compact differential formula for the first derivative of a unit quaternion curve[END_REF]. that for a cubic B-spline curve, no more than 4 basis function B are not null for each knot span[START_REF] Biagiotti | Trajectory Planning for Automatic Machines and Robots[END_REF].For the orientation trajectory, the term ∂η(j B j (u))• D 1 • j B j (u)) • D 2 • n j=i+2 exp(ω j B j (u)), with ω j = log(p -1 j-1 p j ) and D k = ∂exp(•) ∂log(•) ∂log(•) ∂ρ d k , with

  Consider now ρ = [w, v] T = [w, v 1 , v 2 , v 3 ] T ∈ S 3 ,from the definition of log(ρ) it follows that ∂log(

1 2 (

 2 wI 3 -v×) ω = M QP (p) • ω

Therefore, by means of this spring the user will be provided with a perception of the distance from a zero-commanded velocity.

www.haption.com.

ACKNOWLEDGMENTS

This work was supported by the EU H2020 RoMaNS project 645582.


Firas Abi-Farraj

Takayuki Osa

Nicoló Pedemonte

Jan Peters

Gerhard Neumann

Paolo Robuffo Giordano

A Learning-based Shared Control Architecture for Interactive Task Execution

Shared control is a key technology for various robotic applications in which a robotic system and a human operator are meant to collaborate efficiently. In order to achieve efficient task execution in shared control, it is essential to predict the desired behavior for a given situation or context in order to simplify the control task for the human operator. This prediction is obtained by exploiting Learning from Demonstration (LfD), which is a popular approach for transferring human skills to robots. We encode the demonstrated behavior as trajectory distributions and generalize the learned distributions to new situations. The goal of this paper is to present a shared control framework that uses learned expert distributions to gain more autonomy. Our approach controls the balance between the controller's autonomy and the human preference based on the distributions of the demonstrated trajectories. Moreover, the learned distributions are autonomously refined from collaborative task executions, resulting in a master-slave system with increasing autonomy that requires less user input with an increasing number of task executions. We experimentally validated that our shared control approach enables efficient task executions. Moreover, the conducted experiments demonstrated that the developed system improves its performances through interactive task executions with our shared control.

I. INTRODUCTION

Robotic bilateral teleoperation is a key technology for allowing human operators to be in (partial) control of a robotic system in, e.g., hazardous/remote environments such as nuclear sites, deep oceans or the outer space by exploiting a combination of visual and force feedback. However, controlling a complex robotic system using human inputs alone is often difficult and may require special skills or dedicated training [START_REF] Abbeel | Autonomous helicopter aerobatics through apprenticeship learning[END_REF], [START_REF] Franchi | Shared control : Balancing autonomy and human assistance with a group of quadrotor uavs[END_REF]. Assisted teleoperation frameworks, in which the human inputs are 'mediated' by the (partial) autonomy of the controlled robotic systems, have thus been proposed for reducing the workload requirements of the human operator and improving her/his performance [START_REF] Franchi | Shared control : Balancing autonomy and human assistance with a group of quadrotor uavs[END_REF]- [START_REF] Masone | Semi-autonomous Trajectory Generation for Mobile Robots with Integral Haptic Shared Control[END_REF]. Examples in this sense range from virtual fixtures [START_REF] Abbott | Haptic virtual fixtures for robot-assisted manipulation[END_REF], [START_REF] Rosenberg | Virtual fixtures: Perceptual tools for telerobotic manipulation[END_REF], which are taskdependent and require continuous human input, to more complicated shared control schemes aiming at profiting from the human's supervisory capabilities in guiding an autonomous system [START_REF] Masone | Semi-autonomous Trajectory Generation for Mobile Robots with Integral Haptic Shared Control[END_REF], [START_REF] Masone | Interactive Planning of Persistent Trajectories for Human-Assisted Navigation of Mobile Robots[END_REF].

{osa, neumann, peters}@ias.tu-darmstadt.de Finding the proper balance between the level of human intervention and that of robot autonomy remains, however, in most cases an open (and task-dependent) problem. This balance is indeed often tuned heuristically and, moreover, is not adapted during the task execution, a limitation that can lead to an increase of the operator's workload and a decrease of her/his efficiency. On the other hand, a shared control architecture could greatly benefit from the ability to adapt online the operator/autonomy balance as a function of the 'confidence' of the robot autonomy in realizing the shared task. This would (i) allow the operator to intervene only when strictly needed (when the robot autonomy is more likely to fail in fulfilling the task), and (ii) provide the operator with an informative feedback for guiding her/his actions only when the autonomous component is expected to perform well. However, to the best of our knowledge, no established way has yet been proposed for taking a step in this direction.

On the other hand, learning from demonstration (LfD) or programming by demonstration (PbD) have been proposed as an intuitive way to program robotic motions [START_REF] Billard | Robot Programming by Demonstration[END_REF], [START_REF] Argall | A survey of robot learning from demonstration[END_REF]. In LfD methods, the distribution of the demonstrated trajectories is often modeled, and the learned distribution is leveraged for generalizing the demonstrated behaviors [START_REF] Paraschos | Probabilistic movement primitives[END_REF], [START_REF] Osa | Online trajectory planning in dynamic environments for surgical task automation[END_REF]. Recent work on LfD showed that the variance of the demonstrated trajectories can be used to adaptively control the robot behaviors [START_REF] Rozo | Learning physical collaborative robot behaviors from human demonstrations[END_REF]. Leveraging the distribution of the demonstrated trajectories into the design of shared control frameworks seems, therefore, a meaningful/promising approach for obtaining a shared control framework able to adjust online the balance between the robot autonomy and the human preference.

To this end, in this paper we consider that the distribution of the demonstrated trajectories indicates the preference of an expert operator on the robot motion: a high variance in the demonstrated trajectories is assumed to indicate a weak preference of the operator, whereas a low variance is taken as a strong preference. Based on this assumption, we use the variance of the trajectory to control the balance between the controllers autonomy and the human intervention. In particular, when the variance of the demonstrated trajectories is low, the user is fed back with strong force cues meant to minimize any deviation from the 'nominal' (learned) trajectory. On the contrary, as the variance increases, the force cues are suitably attenuated, thus providing the operator with the possibility (and feeling) of freely moving the robotic system along any direction of interest. Through interactive task execution using our shared control framework, we can obtain additional trajectories that are executed under the supervision of the human operator. By aggregating the newly obtained data, we refine the learned trajectory distribution. Therefore, the performance of our shared control system improves through interactive task executions.

This work was conducted in the context of the European H2020 Robotic Manipulation for Nuclear Sort and segregation (RoMaNS) project 1 . In the project scenario, a human operator teleoperates a robotic arm with the aim of approaching and grasping nuclear waste for sort and segregation purposes. The paper particularly tackles the pre-grasp approach phase in the project providing an efficient LfD-based shared control architecture for trajectory following.

The rest of the paper is organized as follows. In Sect. II, an overview on the state of the art is presented. Sec III introduces the general shared control framework and Sec IV details the algorithm for the generation of conditional trajectory distributions from demonstrations, while the details of the shared control architecture are presented in Sect. V. Trajectory refinement from executions is described in Section V-A after which section VI reports the experimental results, and Sect. VII concludes the paper discussing some future directions.

II. RELATED WORK

Shared control appears in various applications ranging from robotic surgery [START_REF] Osa | Hybrid rateadmittance control with force reflection for safe teleoperated surgery[END_REF], to space robotics [START_REF] Kim | Force-reflection and shared compliant control in operating telemanipulators with time delay[END_REF] and maintenance of nuclear sites [START_REF] Schilling | Telerobots in the nuclear industry: a manufacturer's view[END_REF]. Moreover, the use of haptic feedback in assisted teleoperation systems has been proved to be efficient and intuitive for the user. However, as explained, the definition of these haptic cues and the level of the systems autonomy w.r.t. the users authority remain open points in most applications.

The level of autonomy of the assistive system can be defined as a fixed or a variable entity. In fixed assistance systems, the tasks are distributed between the operator and the autonomous controller and cannot be re-assigned [START_REF] Debus | Cooperative human and machine perception in teleoperated assembly[END_REF] whereas in variable assistance systems the task assignment is changing throughout the task execution [START_REF] Dragan | Formalizing assistive teleoperation[END_REF], [START_REF] Kortenkamp | Adjustable control autonomy for manned space flight[END_REF]. This variation is mostly pre-determined and does not take into account the 'confidence' of the autonomous component in 1 http://www.h2020romans.eu/ achieving the task at hand. Moreover, the inputs from the human operator are often isolated from those of the autonomous controller, with each one in charge of controlling different independent variables. In [START_REF] Enes | Blended shared control of zermelo's navigation problem[END_REF], Enes and Book presented a blended shared control algorithm which they tested on a 1dof navigation problem. The users commands were evaluated w.r.t. the optimal solution of the problem and a virtual spring with a constant stiffness was implemented for the haptic feedback. Another blended pseudo-admittance shared control framework for trajectory following has been presented in [START_REF] Abbott | Pseudo-admittance bilateral telemanipulation with guidance virtualfixtures[END_REF]. The virtual fixture was defined around the planned trajectory and a constant-stiffness virtual mass-spring-damper system provided haptic feedback to the operator.

On the other hand, in LfD methods, the distribution of the demonstrated trajectory is often modeled with statistical methods. The framework called ProMP models the distribution of the trajectories in the parameter space [START_REF] Paraschos | Probabilistic movement primitives[END_REF] while the method in [START_REF] Osa | Online trajectory planning in dynamic environments for surgical task automation[END_REF] modeled the distribution of the demonstrated trajectories at each time step using Gaussian Processes. These studies showed that the demonstrated behaviors can be generalized to new situation by modeling the distribution of the demonstrated trajectories. The learned distribution of demonstrations can then be used for designing control schemes. For instance, in [START_REF] Rozo | Learning physical collaborative robot behaviors from human demonstrations[END_REF] the robot learns collaborative skills and adapts its impedance behavior from demonstrations. However, to the best of our knowledge, no prior work has been proposed for leveraging the distribution of the demonstrated trajectories for controlling the balance between the controllers autonomy and the human inputs.

Moreover, although we assume that the demonstrations from human experts are available, it is often expensive to obtain enough demonstrations for building an initial dataset rich enough for capturing all the possible conditions. However, in a shared control framework the trajectory obtained through the actual execution of the task can be also reused to update the model of the trajectory distribution. Calinon et al. described the incremental learning of the trajectory distribution in [START_REF] Calinon | Incremental learning of gestures by imitation in a humanoid robot[END_REF]. The method presented in [START_REF] Calinon | Incremental learning of gestures by imitation in a humanoid robot[END_REF] uses kinesthetic teaching for modifying the trajectory, which can be cumbersome if a manipulator has many degrees of freedom. In contrast, we will use our shared control architecture so that human operator can intuitively modify the trajectory. Regarding online learning, Ross et al. proposed a data aggregation approach and clarified the no-regret property of their approach [START_REF] Ross | A reduction of imitation learning and structured prediction to no-regret online learning[END_REF]. This approach can be used in various applications of imitation learning where additional data of experts' demonstration is available. Chernova et al. proposed to refine the policy by actively requesting additional demonstrations [START_REF] Chernova | Interactive policy learning through confidence-based autonomy[END_REF]. In this method, the confidence of the autonomous agent is computed and if the agent's confidence is low, additional demonstrations are requested. By using this kind of incremental aggregation of the demonstrated data, the policy for generating actions can be improved.

III. PRELIMINARIES

In this work, we consider a bilateral teleoperation system consisting of a 6-dof torque-controlled master device and a velocity-controlled 6-dof slave manipulator arm. Figure 1 illustrates the experimental setup.

Let x M = (p M , φ M ) ∈ R 6 and x S = (p S , φ S ) ∈ R 6 be the position and (a minimal representation of) the orientation of the master/slave end-effectors in their respective base frames. The master device is modeled as a generic (gravity pre-compensated) mechanical system

M (x M )ẍ M + C(x M , ẋM ) ẋM = τ + τ h , (1)
where M (x M ) ∈ R 6×6 is the positive-definite and symmetric inertia matrix (in the 'task' coordinates x M ), C(x M , ẋM ) ∈ R 6×6 accounts for Coriolis/centrifugal terms, and τ , τ h ∈ R 6 are the control and human forces, respectively, which are applied at the master end-effector. Note that the control term τ will be exploited for providing force feedback cues to the user.

As for the slave, we assume instead that its end-effector (where the gripper is mounted, see Fig. 1) can be controlled at the velocity level (as in most industrial manipulator arms) so that

ẋS = ṗS φS = I 0 0 T -1 (φ S ) v S ω S (2) = L(φ S ) v S ω S , (3)
where (v S , ω S ) ∈ R 6 are the available linear/angular endeffector velocity commands and T (φ S ) ∈ R 3×3 is the usual transformation matrix such that

ω S = T (φ S ) φS . (4)
Finally, in the context of this work, we assume that the slave pose x S is directly coupled to the master pose x M (actuated by the human operator via τ h ) modulo a possible constant roto-translation/scaling factor which, w.l.o.g., is here neglected for simplicity. The slave velocity commands are then chosen as

v S ω S = L -1 (φ S )(λ(x M -x S ) + ẋM ) (5)
where λ > 0 is a control gain. We also introduce a context vector s which characterizes the task. For example, the context vector could contain the position of the object we want to grasp. We will use the context vector to adapt the desired trajectories of the masterslave system.

In the next sections, we will discuss how to obtain representative statistics from a set of demonstrated master trajectories x M (t) and how this statistics can then be exploited for designing the force cues τ in (1).

IV. MODELLING DEMONSTRATED TRAJECTORY DISTRIBUTIONS

During the learning phase, we assume that a skilled human operator demonstrates a number of feasible trajectories for letting the slave robot approaching a target object of interest. During this phase the slave is commanded to follow the master position x M (t) as in ( 5), but no force feedback is provided to the user (τ = 0). We then model a distribution of these demonstration trajectories by using indepdent Gaussian distributions for each time step t, i.e.,

p(x M (t)) ∼ N (µ(t), Σ(t)) (6)
where µ(t) and Σ(t) are the mean and variance of x M (t).

Assuming that the trajectory is given as a sequence of states of the system, i.e., ξ = [x M (t 0 ), . . . , x M (T )], the distribution of the trajectory ξ can be modeled as

p(ξ) = T t=0 N (µ(t), Σ(t)), (7)
where T is the number of the time steps of the trajectory. We assume that the demonstrations are available under various contexts s i . In this case, we can model the conditional distribution of the demonstrated trajectories given the context in order to generalize the demonstrated trajectories to new situations [START_REF] Osa | Online trajectory planning in dynamic environments for surgical task automation[END_REF], [START_REF] Da Silva | Learning parameterized motor skills on a humanoid robot[END_REF], [START_REF] Calinon | A tutorial on task-parameterized movement learning and retrieval[END_REF]. Here, we use Locally Weighted Regression (LWR) to model this distribution [START_REF] Cohn | Active learning with statistical models[END_REF], [START_REF] Atkeson | Locally weighted learning for control[END_REF]. Although we use LWR in this work, our approach is not limited to specific regression methods. Other regression methods such as Gaussian Mixture Regression can be also used to model the distribution.

We assume a dataset D = {ξ i , s i } N i=1 of trajectories and context vectors is available. Given a new query context s test , the locality weight for the ith sample can be computed as

w i = exp - (s i -s test ) (s i -s test ) h . ( 8
)
where h is a constant that determines the bandwidth of the locality kernel. As in LWR, we can now compute a local linear model by using a weighted linear ridge regression, i.e.,

a t A t = (S W S + λI) -1 S W X t ,
where the matrix S = [s 1 , • • • , sN ] contains all training context vectors si = [1; s i ] which have been extended by a bias term, the matrix W = diag([w i ]) is a diagonal matrix containing the weightings w i and the matrix

X t = [x 1 M (t), • • • , x N M (t)
] contains all state samples obtained for time step t.

The estimated mean state for time t is then given by

E[x M (t)|s test ] = A t s test + a t . (9)
Similarly, we can compute the conditional covariance over x M (t) given our query context s test using a weighted maximum likelihood estimate, i.e.,

Σ x M |stest (t) = N i=1 w i (x i M (t) -µ i )(x i M (t) -µ i ) N i=1 w i , (10)
where µ i = A t s i + a t is the estimated mean for sample i.

V. SHARED CONTROL GUIDED BY LEARNED TRAJECTORY DISTRIBUTIONS

In our application, we consider the position of the target object p O = (x O , y O , z O ) as the context s upon which the conditional distribution of the trajectory is generated in ( 9) and [START_REF] Paraschos | Probabilistic movement primitives[END_REF]. To simplify the notation, we will refer to E[x M (t)|s test ] by x M,d (t) = (p M,d (t), φ M,d (t)) in the following section. Moreover, we will denote by Σ p (t) ∈ R 3×3 and Σ φ (t) ∈ R 3×3 the 3×3 block diagonal elements of the covariance matrix Σ x M |stest (t) ∈ R 6×6 computed in [START_REF] Paraschos | Probabilistic movement primitives[END_REF].

Having estimated the distribution from demonstrations, we now present the design of the force cues τ in (1). Our approach is as follows: we treat the mean of the conditional distribution x M,d (t) as a desired trajectory for the master device. Indeed, this trajectory represents, in some sense, the 'best/nominal' approaching trajectory (to the considered target location) from the expert user's demonstrations collected during the learning phase. The force cues τ will then attempt to steer the master device along the desired x M,d (t) with a 'stiffness', roughly speaking, inversely proportional to the variance of the generated conditional distribution. This will effectively tune the degree of maneuverability of the master device around the nominal trajectory x M,d (t), and, thus, provide the operator with an increased situational awareness (more freedom to deviate from x M,d (t) for large variances, and the converse for small variances).

Let τ = (τ p , τ φ ), where τ p ∈ R 3 and τ φ ∈ R 3 are the forces/torques acting on the position/orientation p M , φ M of the master end-effector, and define e p = p M,d -p M and e φ = φ M,d -φ M as the position/orientation errors. Following the classical literature on task-space impedance control [START_REF] Caccavale | Six-dof impedance control based on angle/axis representations[END_REF], we then design

τ p T (φ)τ φ = M p 0 0 M φ pM,d φM,d + Bp 0 0 B φ ėp ėφ + Kp 0 0 K φ ep e φ (11)
where M p (p) and M φ (φ) are the 3 × 3 block diagonal elements of the master inertia matrix M (x M ) in (1) associated to the coordinates p and φ, (B p , B φ ) and (K p , K φ ) are 3 × 3 damping and stiffness matrix terms, and T (φ) is the transformation matrix defined in (4).

As mentioned earlier, a decrease in the variance of the generated distribution is assumed to indicate more confidence in the generated trajectory. This confidence is to be reflected as an increase in the stiffness of the virtual spring and the force cues fed to the human operator and vice versa. To this end, we consider the eigenvalue decomposition of the (symmetric and positive definite) covariance matrixes

   Σ p = V p S p V p , Σ φ = V φ S φ V φ , (12)
with S p = diag(σ pi ) and S φ = diag(σ φi ). The desired stiffness matrices (K p , K φ ) are then defined as

K p = V p K p,0 e -αpSp V p K φ = V φ K φ,0 e -α φ S φ V φ (13)
where K p,0 = diag(k p,i ) > 0, K φ,0 = diag(k φ,i ) > 0, α p > 0, and α φ > 0. This stiffness design achieves the desired behavior: indeed, by focusing on the first position term (the second one being equivalent), the chosen K p will implement a virtual spring of value k p,i e -ασp,i on each of the principal axes of Σ p (t). The stifness will then range from the maximum values k p,i for small variances σ p,i ≈ 0, to negligible values for large variances σ p,i , with the parameter α p governing the decrease rate.

Finally, the damping terms are designed, as usual, in order to obtain a critically-damped closed-loop behavior [START_REF] Gray | A simple calculation for the critical damping matrix of a linear multidegree of freedom system[END_REF]

B p (t) = 2(M -1/2 p K p (t)M -1/2 p ) 1/2 B φ (t) = 2(M -1/2 φ K φ (t)M -1/2 φ ) 1/2 . ( 14
)

A. Trajectory Refinement through Interactive Task Executions

By using the shared control architecture described in Section V, we can obtain a new sample of the trajectory and the context. The obtained sample can be used to refine the model of the trajectory distribution by simply aggregating it to the dataset. When a new trajectory is obtained, we examine the information gain which can be obtained by adding it to the dataset. Using the dataset D, we model the joint distribution of the context and the state of the system at time t as a Gaussian distribution

p D s x M (t) ∼ N D (µ D (t), Σ D (t)). ( 15
)
Let D denote the dataset which can be obtained by aggregating the data as D = D ∪ D new . The information gain from the newly obtained data is given by Kullback-Leibler divergence [START_REF] Kullback | On information and sufficiency[END_REF] as

I(D, Dnew) = T t=0 DKL(p D ||pD) = T t=0 DKL(N D ||ND) = 1 2 T t=0 log det ΣD(t) det Σ D (t) -n + tr(Σ -1 D (t)Σ D (t)) +(µD(t) -µ D (t) ) Σ -1 D (t)(µD(t) -µ D (t)) , ( 16
)
where n is the dimension of [s , x M (t)]. If the information gain I(D, D new ) is larger than the threshold I 0 , the newly obtained data is aggregated to the dataset as D ← D ∪ D new . Using the information criterion, we can keep the dataset as compact as possible. Our approach for learning shared control from interactive task executions is summarized in Algorithm 1.

Through interactive learning, the trajectory distribution is adapted by the operator's preference. If the operator's preference is stationary, the trajectory distribution induced by the learned model converges to the trajectory distribution induced by the operator as the number of the trajectory samples increase. Therefore, the required amount of control input from the operator is expected to decrease as the learned trajectory distribution is more and more refined.

VI. EXPERIMENTAL RESULTS

We evaluated our approach using the experimental testbed shown in Fig. 1. The slave side is an Adept Viper s850 6-dof serial manipulator equipped with a gripper whereas the master consists of the Haption VIRTUOSE 6D haptic device 2 .

In the considered task, the human operator is required to steer the slave arm towards a target object with the intention of grasping it. The target object is assumed to be fixed and the slave and the master arms go back to their respective pre-defined initial positions after every iteration. A video is attached to this paper showing the proposed approach, the test bed the performed experiments. A human operator demonstrated the described task 20 times using the master-slave system. During the demonstrations, the master manipulator was totally compliant and the user did not receive any force feedback.

To evaluate our algorithm, we used data from demonstrations performed under biased conditions. This bias leads to the generation of 'non-optimal' trajectory distributions given certain contexts which is essential in order to induce a significant analysable intervention from the human operator. To introduce the bias, initial demonstrations were performed with the target object placed 16 cm lower than in the testing phase (Fig. 2). A human operator performed the task using our shared control framework, and the learned trajectory distribution was refined by aggregating the executed trajectory to the dataset after every iteration. This procedure was repeated five times.

cm

Fig. 2. The experimental setup. The initial demonstrations were performed with an object on a table. The learned trajectory distribution was then tested with the object on a box. The initially demonstrated trajectories are shown in Fig. 3. To avoid redundancy, the displayed plots correspond only to the translational position of the end effector although both, positions and orientations of the trajectories, were planned and executed as described in the previous sections.

Fig. 4 shows the planned trajectory distribution (plotted in red with the shaded region reflecting the value of the variance) versus the executed trajectory (in blue) for the first, third and last iterations of the experiments. The difference between the planned trajectory distribution and the trajectory executed under the human operator's supervision was large in the first iteration as shown in Fig. 4(a) where the executed trajectory is mainly monotone while the planned distribution shifts from decreasing to increasing as in x and z. Moreover, the planned trajectory was jerky with lots of vibrations due to the prediction uncertainty. Meanwhile, Fig. 4(b), which corresponds to the third iteration of the experiment, shows a notable improvement in the behavior of the planned trajectory distribution which is now more in line with the executed trajectory. The planned trajectories are, as expected, even better in the fifth iteration (Fig. 4(c)). Although the offset between the planned and executed trajectories still remained after five task executions, we think these are acceptable results. As long as the operator holds the master manipulator, an unintentional force is applied due to the inertia of the operator's hand. However, since the operator did not try to modify the trajectory in the final two iterations, we consider that the offset is acceptable. On the other hand, the smoothness of the trajectory has significantly improved when comparing the planned trajectory generated during the fifth iteration with the one generated in the first. This is mostly visible in x and z since the displacement in the y-axis was limited in the described experiment. Metrics of the intervention of the human operator in each of the 5 iterations of the experiment are shown in Fig. 5. The magnitude of the forces and the torques exerted by the human operator at the master side is clearly decreasing with the progression of the experiment. However, as a result of the continuous interaction between the human operator and the end effector of the master arm, these forces will converge to a range but not to zero. The slight increase in the linear force for iteration 4 is noise resulting from the mentioned interaction.

This result illustrates the efficiency of the shared control architecture in providing the human operator with the needed informative force cues reflecting his deviation from the generated trajectory distribution. Moreover, it reflects the adaptability of the learning routine and its swift compliance over iterations with the inputs dictated by the human operator resulting in less intervention from the human operator over subsequent task executions.

In addition, the information gain from each executed trajectory (defined in ( 16)) was analyzed in this experiment as well to check its effectiveness as a token in deciding whether to aggregate the newly executed trajectory to the learning dataset or not. Fig. 6 recounts the behavior of the indicator over time showing a decrease in the information gain with every iteration. The horizontal line signifies the threshold above which an execution is aggregated to the dataset. However, for the sake of testing the impact of this aggregation, all the executions were exceptionally aggregated to the dataset in this experiment.

The mentioned figure shows that the most significant information gain was achieved during the first three iterations after which it dropped significantly. This result is in line with Fig. 4 which depicts the planned and executed trajectories during the first, third and fifth iterations. The figure shows that the planned trajectory changed significantly between the first and the third iterations. In contrast, the change was more limited between the third and the fifth. We can conclude that the aggregation of the last two executions (for which the information gain was below the threshold in Fig. 6 ) into the dataset had only little impact on the final results. The observed behavior thus validates the effectiveness of the chosen measure for informative training samples.

VII. CONCLUSION AND DISCUSSION

We presented a shared control framework that is guided by a trajectory distribution learned from human experts. By exploiting the learned distributions, we can adaptively control the balance between autonomy of the controller and the human intervention. We used the variance of the demonstrated trajectories as an indicator of the human experts' preference, and the haptic feedback exerted at the master system was controlled accordingly. Experimental results show that the learned trajectory distribution was refined and the performance of our shared control improved through interactive task executions. In future work, we will analyze the stability of the shared control architecture and evaluate the efficacy of our system by performing tests with subjects.

In the experiments, we showed that our shared control framework learns the demonstrated behaviors through interactive task executions. However, to further evaluate the efficacy of our shared control framework we will perform tests with subjects on more complex tasks in future work. Moreover, although we used Euler angles to represent the orientation, unit quaternions or other singularity-free representations can be used. However, the representation with unit quaternion requires modeling the distribution with unit norm constraints, which is not well-investigated in literature and would require an extension of the learning method.

So far, we addressed the shared control for a single task, e.g. approaching objects. It is interesting to address the problem for more complicated tasks consisting, for example, of a sequence of primitive motions. Another potential extension of the shared control algorithm is the incorporation of visual information, as in [START_REF] Farraj | A visual-based shared control architecture for remote telemanipulation[END_REF], to improve the robustness of the system towards loose joints and miscalibrations. The stability of the teloperation loop is to be analyzed as well. In particular, the presence of a time-varying stiffness needs to be analysed even though no instabilities were reported during the experiments.

Fig. 1 .

 1 Fig. 1. The experimental setup showing the slave robotic arm on the left and the master haptic arm on the right.

Algorithm 1

 1 Learning Trajectory Distribution through Interactive Task Executions Input: dataset of the trajectories demonstrated by experts and the contexts of each demonstration D, information gain threshold I 0 repeat Model the trajectory distribution p(ξ|s) Update the parameters of the shared control Perform the task under the context s new using the shared control Record the obtained data D new = {ξ, s new } Evaluate the information gain from the new data I(D, D new ) if I(D, D new ) > I 0 then Aggregate the dataset D ← D ∪ D new end if until the trajectory distribution learned

Fig. 3 .

 3 Fig. 3. The three positional components of the pose of the end effector of the master arm at every time step t during each of the 20 demonstrations

Fig. 4 .

 4 Fig. 4. Improvement of the trajectory prediction. Blue lines represent the executed trajectory, and red lines represent the distribution of the predicted trajectory. (a)The executed trajectory and the predicted trajectory based on the initial dataset. (b)The executed trajectory and the predicted trajectory after the aggregation of three additional trajectories obtained from the interactive task executions. (c)The executed trajectory and the predicted trajectory after the aggregation of five additional trajectories obtained from the interactive task executions.

Iteration

Fig. 5 .

 5 Fig. 5. Metrics of the applied human intervention during task execution. The intervention from the human operator decreased after interactive task executions. (a)Mean force exerted at the master manipulator. (b)Mean torque exerted at the master manipulator.

Iteration

Fig. 6 .

 6 Fig. 6. Information gain of the additional data. The information gain decreases as the number of the data samples increases.

F. A. Farraj, Nicoló Pedemonte, and P. Robuffo Giordano are with CNRS at Irisa and Inria Rennes Bretagne Atlantique, Campus de Beaulieu, 35042 Rennes Cedex, France. {firas.abi-farraj, nicolo.pedemonte, prg}@irisa.fr

T. Osa, J. Peters, and G. Neumann are with Institut für Intelligente Autonome Systeme, Technische Universität Darmstadt, 64289, Germany.

J. Peters is with Max-Planck-Institut für Intelligente Systeme, Tübingen, 72076, Germany.

www.haption.com

ACKNOWLEDGMENT

This project has received funding from the European Unions Horizon 2020 research and innovation programme under grant agreement No #645582 (RoMaNS).