Whole-Body Teleoperation and Shared Control of Redundant Robots with Applications to Aerial Manipulation

This paper introduces a passivity-based control framework for multi-task time-delayed bilateral teleoperation and shared control of kinematically-redundant robots. The proposed method can be seen as extension of state-of-the art hierarchical whole-body control as it allows for some of the tasks to be commanded by a remotely-located human operator through a haptic device while the others are autonomously performed. The operator is able to switch among tasks at any time without compromising the stability of the system. To enforce the passivity of the communication channel as well as to dissipate the energy generated by the null-space projectors used to enforce the hierarchy among the tasks, the Time-Domain Passivity Approach (TDPA) is applied. The efficacy of the approach is demonstrated through its application to the DLR Suspended Aerial Manipulator (SAM) in a real telemanipulation scenario with variable time delay, jitter, and package loss.


Introduction
Kinematically redundant robots present significant advantages. In addition to allowing manipulation tasks to be performed in an optimal manner, additional lower-priority tasks can be incorporated through suitable control design in order to achieve multiple objectives [33]. For that reason, such robotic systems have been employed in different fields, e. g., aerial manipulation [21], assistive robotics [23], and robotic surgery [46].
In order to benefit from the versatility of redundant robots and simultaneously exploit perception and cognition capabilities of a human operator, such systems have also been applied in the field of teleoperation. While the endeffector pose is commonly chosen as the main task to be teleoperated [34], the developed approaches differ in how additional tasks are commanded. In [41] and [29], the endeffector of a redundant manipulator is teleoperated while the internal motion is autonomously controlled in order to maximize the performance of the main task. Moreover, in [37] the redundant null-space motion is used for dissipating energy and maintaining the overall passivity of an endeffector teleoperation task.
The concept of having lower-priority tasks being autonomously performed can significantly reduce the cognitive load on the human operator. However, in some applications, it might be necessary that the user occasionally takes control of a lower-priority task. An example thereof can be found in the field of aerial telemanipulation, where a video camera is attached to the flying base of the robot [13]. For such systems, it is useful to allow the human operator to steer the camera as a secondary task and see the objective from different angles without disturbing the endeffector. Furthermore, this could also allow the operator to align the camera frame with the action frame, i. e., such that the directions of motion of the input device align with the motion of the teleoperated robot in the camera frame.
Even though works like [1] and [20] present solutions for allowing the operator to switch between base and end-effector teleoperation in mobile manipulation tasks, additional care has to be taken if a hierarchy between the tasks is to be fulfilled. On the other hand, a framework for allowing multiple master devices to control a redundant slave robot in a hierarchical manner was presented in [30]. There, one of the masters is responsible for commanding the main end-effector task while the other commands a secondary task. This is achieved by applying velocity-level redundancy resolution and projecting the velocity of the second master onto the null-space of the Jacobian of the primary task. However, the limitation of that approach lies in the fact that task decoupling only at kinematic level is achieved while lower-level tasks can still disturb the main one at dynamic level. Adding to that, the approach is restricted to applications where the communication delay between the local and remote sites is negligible.
On the other hand, in order to tackle the problem of instability caused by time delay, package loss, and jitter in bilateral teleoperation, the Time Domain Passivity Approach (TDPA) [42] is a reliable choice due to its passivity, robustness, and position synchronization characteristics. TDPA has been widely used in the fields of space teleoperation [4], force control [8], series elastic actuators [27], time-delayed cooperative control [32], and others. Its model-free characteristics allows it to perform better in comparison with other passivity-based approaches [6] while possessing a significantly simpler structure than fuzzy-logic and neural-network based approaches (e. g. [48] and [47]). Nevertheless, in order to allow its application in a hierarchical shared-control scenario, the TDPA framework has to be systematically adapted.

Main Contribution
In the aforementioned scope, this work presents a novel passivity-based framework to allow hierarchical wholebody bilateral teleoperation and shared control of redundant robots. The proposed method gives the human operator the possibility of commanding different tasks of a remote robot through one or more master devices while being able to switch among tasks and receiving relevant haptic feedback about the task being commanded while the other tasks are autonomously performed.
Our approach can be seen as an extension of the traditional position-force (P-F) teleoperation architecture for allowing simultaneous multi-task teleoperation and autonomous control of redundant torque-controlled robots. In addition, the null-space wall concept [13] is incorporated to ensure that the operator receives not only information about interactions with the environment, but also about the limits of the workspace of the commanded task, imposed by the aforementioned hierarchy.
An overview of the proposed concept is depicted in Fig. 1, where the human operator commands different subtasks of a redundant robot while receiving not only haptic information about end-effector contacts, but also about the limits of other secondary tasks. In addition, if a camera is attached to the robot, the operator is allowed to actively adjust its pose in order to receive a desired view from the remote environment.
This paper extends our previous work, [12] and [13], in two directions. Firstly, it provides the extension of the multi-DoF drift compensator presented in [12] to compensate for the drift caused by TDPA in all subtasks, not only the main one, as shown in that paper. In addition, the framework presented here can be seen as a generalization of the one presented in [13], where the following points were improved: the task selection logic, which allows the user to switch among tasks, was modified in order to make the system more robust to time delay and allow online switching among tasks; moreover, an analytical treatment of the effects of the coordinate transformation performed by the whole-body controller and its effects on the passivity of the system is presented. With that, the approach presented here is proven to allow for passive bilateral teleoperation of arbitrary levels, simultaneous or individually, in the presence of time-delays, jitter, and package loss.

Structure of the Paper
This paper is structured as to present the composing elements of the proposed framework, which are: -Dynamics of the kinematically-redundant remote robot and its local passive hierarchical controller; -Communication channel with passivity controllers and a drift compensator; -Input device with online switching among tasks.
In that scope, the paper is divided as follows: Section 2 introduces mathematical notations and definitions, which will be used throughout the paper. Section 3 provides an overview of the time-domain passivity theory for both one and two-port systems. Section 4 presents the local hierarchical controller of the slave robot. Those three sections provide the reader with the necessary theoretical background for the paper. Moreover, additional effort has been made as an attempt to define a common notation for the TDPA and hierarchical whole-body control approaches.
The main contribution of the paper is introduced in Section 5, which presents the proposed bilateral wholebody teleoperation and shared-control framework and its composing elements, as well as providing an analysis about its passivity properties. Moreover, Section 6 shows results of the application of the proposed to teleoperation of a redundant aerial manipulator. Finally, Section 7 summarizes and concludes the paper.

Notations and Definitions
This section introduces useful notations and definitions, which will be used throughout the paper, especially those related to the Special Euclidean SE(3) group and its Lie algebra.

The Special Euclidean Group and its Lie Algebra
The pose of a rigid body in space can be described by a matrix representation of the SE(3) Lie group, which is written as where p is a position vector in R 3 and R is a rotation matrix, belonging to the Special Orthogonal SO(3) group, whose Lie algebra is so (3). Furthermore, the velocity of a rigid body can be expressed by elements of the Lie algebra of SE(3), namely se (3), as where · indicates the skew-symmetric operator applied to a vector in R 3 , and ω, v ∈ R 3 are angular and linear velocities, respectively. Adding to that, due to the isomorphism between se(3) and R 6 , it is useful to define the operators [·] ∧ : R 6 → se(3) and [·] ∨ : se(3) → R 6 , such that the velocity of a rigid body can be expressed as V = v T ω T T ∈ R 6 , which can be represented in body ( B V ) or in spatial frame ( S V ) [31].

Dynamical Systems on SE (3)
A dynamical system with state g ∈ SE(3) evolves according to the following differential equation in continuous time [9] whose recursive solution (assuming piece-wise constant velocities) in discrete time, given a set of initial conditions, can be approximated to where ΔT is the sampling time and k is the discrete-time index, such that, with slight abuse of notation f (k) f (kΔT ) (14) for some function of time f (t).
Hereafter, unless otherwise stated, all velocities will be assumed to be body velocities and the superscript B (·) will be dropped for simplicity of notation.

Relative Pose and Relative Velocity
For control purposes, it is sometimes necessary to define a relative pose between two SE(3) frames (e. g., a desired and an actual pose). This element can be defined as where g des and g are the desired and actual poses, respectively. Accordingly, the body velocity, which corresponds to the relative pose is where V and V des are the body velocities related to g and g des , respectively, and Adg−1 is defined as [31]

Time-Domain Passivity
The Time-Domain Passivity Approach (TDPA) is a widely used method, developed in order to enforce stability of both one-port [22] (e. g., haptics) and two-port [42] (e. g., bilateral teleoperation) setups where velocity and force signals are exchanged. In teleoperation systems, time delay and package loss introduced by the communication channel might compromise the overall stability of the system, whereas in haptics or other one-port systems instability can be the result of the sampling and quantization of passive controllers or the application of non-passive ones. In contrast to methods where a damping element is designed for the worst case scenario (e. g., [2,35]), TDPA consists in adding adaptive damping components in order to dissipate only the necessary amount of energy, computed using measurements of the forces and velocities being exchanged. For that reason, it seems to feature better performance in comparison to other approaches [6].
In order to facilitate the understanding of TDPA and its application, the general passivity concept is presented in Section 3.1.

Passivity-related Definitions
Definition 1 (Passivity) A dynamical system H with input u and output y is said to be passive if where is called the supplied energy of system. Without loss of generality, E(0) can be assumed to be zero while E(t) is evaluated as the integral of the power exchanged at the port < u, y >, computed as the inner product between input and output, as shown in Eq. 19. For discrete-time dynamical systems, the supplied energy of a port < u(k), y(k) > can be computed as (20) Lemma 1 (Supplied Energy of Feedback Systems) Let H 1 and H 2 be dynamical systems with input-output pairs < u 1 , y 1 > and < u 2 , y 2 >, respectively. Let H be the dynamical system formed by the feedback interconnection of H 1 and H 2 , as shown in Fig. 2. The supplied energy E(k) of the port < u, y > of H is where E 1 (k) and E 2 (k) are the supplied energies of the ports < u 1 , y 1 > and < u 2 , y 2 >, respectively. Proof From the block diagram in Fig. 2, the following equations can be derived. Therefore, which, through Eq. 20, implies in Eq. 21.
An important corollary can be derived from Lemma 1 and Definition 1, as follows.

Corollary 1 (Passivity of Feedback Systems)
The feedback interconnection of passive systems is passive [24].

One-Port TDPA
In some occasions, as will be seen in Section 4.5, it is desired to enforce one-port passivity of dynamical systems.
In those cases, an extra feedback loop must be included, such that, according to Lemma 1, the original port becomes passive.
According to Lemma 1, a dynamical system H 1 can have its input subtracted by the output of the system H pc (corresponding to H 2 in Fig. 2) with input-output pair < y, y pc > in a feedback loop, such that the original port < u, y > becomes passive. The system H pc is known in the TDPA literature as the passivity controller (PC), whose law will be described subsequently.
In this scenario, Eq. 21 can be rewritten as where E 1 (k) is the energy of the system being passivated by the PC and E pc (k) is the energy of the PC itself, which can be written in a recursive way according to Eq. 20, such that Eq. 26 becomes where y pc (k) is responsible for ensuring that E(k) ≥ 0 by increasing the value of E pc (k) whenever necessary. For that purpose, a passivity control law for y pc can be defined as where Γ (k) is a positive-definite square matrix and λ(k) is a non-negative scalar, defined as where W (k) is known as the passivity observer (PO), defined as Moreover, It can be seen that, for the time steps where W (k) ≥ 0, no action is taken by the PC, which ensures that E(k) = W (k) ≥ 0 and the input is not modified, i. e., u 1 On the other hand, if W (k) < 0, the PC modifies the original input (u 1 (k) = u(k) − y pc (k), s. t. y pc (k) = 0), in order to enforce that E(k) = 0 and maintain the passivity of the original port < u, y >.

Two-Port TDPA
In addition to enforcing the passivity of one-port systems, TDPA can also be applied to two-port systems, like the communication channel in teleoperation [42]. The details of this application will be shown in the following subsections.

Position-Force Architecture
One of the simplest and most common teleoperation schemes is the P-F architecture [26], where the master velocity is sent through the communication channel and serves as the desired velocity to the slave. In turn, the force computed by the slave-side controller is sent back to the master. A block diagram of the P-F architecture can be seen in Fig. 3. There, T f and T b are the forward and backward where the index k, representing the time-variable nature of the time delays, will be dropped hereafter. Furthermore, the terms V m and V s refer to the velocities of the master and slave devices, respectively, andV sd is the delayed master velocity given as the reference to the slave controller. Moreover, F s is the force exerted by the slave controller andF m is its delayed version, applied to the master device. Furthermore, F h and F ext are the forces applied by the human operator on the master device and by the environment on the slave robot, respectively. Moreover, a network representation of the P-F architecture is depicted in Fig. 4, where the block CC represents the communication channel, M+H is the master's side network including the master device and the human operator, and S c is the slave's side network, composed of slave robot, local controller, and environment.
As stated in [42], a sufficient condition for the teleoperation system to be passive is that the networks M+H and S c are one-port passive and the communication channel is two-port passive. However, the communication channel between master and slave not only can introduce time delay but also package loss and jitter. Those three phenomena are known for causing an undesired effect, namely, the instability of the teleoperation system. Therefore, additional steps have to be taken in order to enforce the passivity of the channel.

Time Delay Power Networks
In bilateral teleoperation setups, the communication channel is usually represented by one or more Time Delay Power Networks (TDPNs) [42], which are two-port networks that exchange velocities and forces. In addition to constant or variable time delays, TDPNs can also model package loss in the signals being transmitted. Figure 5 shows the signal flow of the TDPN. The terms E M and E S represent the energies computed on the master and slave sides, respectively. The in and out subscripts are used to represent the direction of by taking their sign into account, as follows.
A sufficient condition for the passivity of a TDPN is that observed on the right and left-hand sides of the TDPN, respectively. Note that, E M in (k) and E S in (k) are monotonically increasing functions, which results in their delayed values being upper bounded by the actual ones. Therefore, the conditions described in Eqs. 40 and 41 are able to guarantee the passivity of the TDPN even in the presence of time delays, package loss, and jitter [42].

Passivity Observer
As opposed to the one-port TDPA, for the two-port approach the passivity conditions are Eqs. 40 and 41, and the PC only needs to regulate the energies flowing out of the channel, computed as in Eqs. 37 and 39. Therefore, the POs on each side of the channel can be defined as

Passivity Controller
In order to fulfill the passivity conditions from Eqs. 40 and 41, the slave-side PC can be applied as [12] where Furthermore, Γ s (k) is a positive-definite weighting matrix and λ s (k) is a non-negative scalar. The velocity removed by the PC from the delayed master velocity in order to keep the system is given by and the resulting velocity used as a reference by the slave is given by assuming all velocities are represented in the same frame. Likewise, the master-side PC can be applied as [37] where Moreover, Γ m (k) is a positive-definite weighting matrix and λ m (k) is a non-negative scalar. The force removed by the PC from the delayed slave force in order to keep the system passive is given by and the resulting force applied to the master device is given by The key idea of the aforementioned PCs is that they saturate E M out (k) and E S out (k), setting their upper bounds as

Task-Space Dynamics and Control of Redundant Robots
This section describes an approach used in order to allow hierarchical control of redundant slave robots, which will be subsequently appended to the full teleoperation scheme.

Joint-space Dynamics
The dynamics of a fully-actuated robot with n joints can be written as where q,q,q ∈ R n is a set of generalized coordinates and their corresponding first and second time derivatives, M(q) ∈ R n×n is the inertia matrix, C(q,q)q ∈ R n is a vector of Coriolis and centrifugal generalized forces, formulated such that [31] Moreover, G(q) ∈ R n is the gravitational generalized-force vector. The terms τ ∈ R n and τ ext ∈ R n are the generalized control and external forces, respectively.

Task Velocities
A set of r ∈ N task velocities can be defined as where V i ∈ R m i are task velocities of dimension m i and J i (q) ∈ R m i ×n is the Jacobian of the mapping from generalized velocities to the respective task-i velocities. Furthermore, it is assumed that, Using the above definitions, an augmented Jacobian [45] can be defined as . . .
where J aug i (q) is assumed to keep full row rank during the performed tasks. Moreover, the augmented task velocities can be defined as ⎡ In order to define a set of velocity coordinates, where lower-priority (greater i) task velocities and accelerations do not disturb higher-priority ones, dynamically consistent null-space projection matrices [16,25] can be defined as where I is the identity matrix and (·) M + represents the inertia-weighted pseudoinverse [17] of the augmented Jacobian matrix. Using the aforementioned projections, the following Jacobian matrices can be defined.
such that a set of decoupled task velocities can be defined as ⎡ where V is the augmented decoupled task velocity, whose map from generalized velocities isJ (q).
In addition, based on Eqs. 57 and 60, the mapping between original task velocities, V , and decoupled task velocities, V , can be defined as [16] where i. e., B (q) is block lower triangular, with identity matrices as main diagonal elements. This confirms the hierarchy imposed in the velocity coordinates V , which ensures the absence of bottom-up disturbances among task velocities.

Task Forces
Similarly to the set of task velocities, the generalized control and external forces can be transformed into forces of two kinds, as follows.
such thaṫ anḋ Moreover, a relationship between forces in both coordinates can be derived as

Decoupled Dynamics and Decoupling Controller
Having defined a set of decoupled velocities in Eq. 60, the dynamics in Eq. 52 can be transformed into the new coordinates as where i. e., Λ(q) is block diagonal. Moreover, The fact that μ(q,q) is usually fully occupied is responsible for allowing power-conserving inter-task interference at velocity level. In order to avoid that phenomenon as well as compensating for gravity, a preliminary compensation action can be applied as follows.
where τ μ is responsible for transforming μ(q,q) into a block diagonal matrix. After applying the compensation terms, τ c becomes the effective control signal, which will be responsible for accomplishing the desired tasks in a hierarchical manner. By compensating for the crosscoupling terms in μ(q,q), decoupled dynamic equations for each task can be derived as

Local Slave Controller
In usual telemanipulation tasks, a controller is implemented on the slave robot in order to ensure that the reference, given by the human operator through the master device, is followed. As desired acceleration signals are not available and due to the natural capacity of the human operator to compensate for steady-state errors, a tracking controller is usually not necessary for telemanipulation tasks. Therefore, a proportional-derivative (PD) controller that regulates the error between commanded and actual pose as well as their relative velocity to zero is typically applied.
For tasks that comprise the full pose of a point of interest on the manipulator, an error element between the desired and actual poses on SE(3) can be defined as in Eq. 15 and its body velocity as in Eq. 16. Moreover, a body-frame PD controller can be applied as follows [39,49].
where K T ∈ R 3×3 , K R ∈ R 3×3 , and K D ∈ R 6×6 are symmetric, positive-definite matrices. Furthermore,η and˜ are the scalar and vector parts of a quaternion representation ofR, respectively, and E(η,˜ ) =ηI − ˜ . In case the task does not comprise the full pose, F P D can be adapted accordingly (e. g., implementing only rotational or translational controllers or parts thereof).
In order to implement a hierarchical PD controller, as a special case of the compliance controller presented in [38], the control law in Eq. 74 can be implemented for each task as F c,i = F P D (g i ,Ṽ i ), taking into account the dimension of each task.
Nevertheless, it can be noted that, despite being implemented as a force in the decoupled coordinates, F c,i , the controller depends on original task velocities V . Despite being necessary to maintain the task hierarchy, this leads to an undesired effect, namely, the loss of passivity of the port <Ṽ i , F c,i >, which would, otherwise, be ensured by a PD controller in original task velocities. A discussion on that matter and a solution to that issue will be presented in Section 5.4.

Whole-Body Bilateral Teleoperation of Redundant Robots
This section describes the composing elements of the proposed framework to allow for time-delayed bilateral teleoperation and shared control of redundant robots in a hierarchical manner.

Null-space Wall
Although F c is not directly applied to the robot, but rather mapped to τ c through the mapping presented in Eq. 64, it carries out important information about the limits of the null space of higher-priority tasks. In case a task i ≥ 2 is commanded, the operator directly commands g i,des or V i,des , respectively. However, in case a position that is not reachable without affecting the higher-priority tasks is commanded, the control force F c,i (or parts of it) will be projected to zero. If the operator keeps commanding V i,des towards that direction, the errors between desired and current pose in Eq. 74 will increase. As a consequence, an increase in F c,i will be observed. This behavior resembles what happens when the robot being teleoperated hits a physical wall. Therefore, by sending F c,i as haptic feedback, the operator will have the feeling as if the robot were trying to penetrate a wall when an unfeasible pose is commanded to the desired task level. The stiffness of that wall will be initially defined by the gains of the PD controller, but can be scaled according to the task requirements.
On the other hand, if the pose commanded by the operator is reachable without disturbing the higher-priority tasks, the operator will perceive lower forces, which are due to the imperfect tracking capabilities of the controller, as is usually the case for the P-F architecture in teleoperation. Therefore, for each robot configuration there would be a region where the operator will perceive the robot in freespace motion and another where something similar to a wall contact will be perceived, which is the case when the commanded pose is not feasible due to the imposed hierarchy.
It is important to remark that, without the aforementioned haptic feedback, the operator would have to rely on visual feedback in order to know if the lower-priority tasks are following the commands. This could be particularly complex in aerial-manipulation tasks since (a) the camera is usually attached to the body of the robot and parts of it (e. g., the flying base in aerial manipulation) do not appear in the frame, (b) it is hard to distinguish relative from absolute motion between the tasks in the camera images. On the other hand, the wall-contact feeling perceived by the operator would indicate that the desired direction of motion is not feasible. Adding to that, the use of the aforementioned force feedback does not prevent the user from feeling contacts with physical walls, which would also generate an increase in F c .
In summary, in order to implement the null-space wall concept in a P-F architecture, the feedback force F s , in the teleoperation framework presented in Section 3.3, is defined as Note that the use of F c as the feedback force would also provide the user with environment-contact forces. However, no information about the limits of the secondary tasks, i. e., the null-space wall, would be provided.

Whole-body Bilateral Teleoperation Framework
Using the aforementioned null-space wall concept, the bilateral teleoperation scheme can be arranged as shown in Fig. 6. All forces and velocities are of dimension n, i. e., the forces and velocities of all tasks are transferred between master and slave. The block M+H is the interconnection Fig. 6 Overall whole-body teleoperation framework of the master device and the human operator, also shown in Fig. 4, which contains a task selection functionality in order to allow the human operator to command one task at a time or a number of them simultaneously, and switch between tasks as needed, as will be described in Section 5.3. Moreover, S c (also in Fig. 4) represents the controlled slave robot together with the environment, which will be described in Section 5.4. The blocks P C M and P C S represent the master and slave PCs, respectively, discussed in Section 5.5, and DC is the drift compensator presented in Section 5.6.
The proposed framework allows for the passive teleoperation of all tasks, either simultaneously or separately, while the remaining tasks are autonomously performed. A discussion about its overall passivity will be provided in Section 5.7.

Task Selection
The internal view of the block M+H from Fig. 6 is depicted in Fig. 7. The matrix Ω ∈ R p×n , where p ≤ n is the number of DoF of the master device, is a selection matrix (composed of ones and zeros), whose structure can be Fig. 7 Master network overview directly commanded by the operator, according to the tasks to be teleoperated. Moreover, M represents the dynamics of the based in the err, and H represents the dynamics of the human operator, which is assumed to passively interact with the master device.
Within the proposed framework, task selection occurs locally and the uncontrolled tasks have their velocities commanded to zero. Thus, tasks that are not being controlled will have the last commanded pose as desired while the actual pose could move in order to allow the fulfillment of higher-priority tasks. This approach also has the advantage of providing the master with the capability of switching among tasks while having relevant force feedback about the desired task instantly available, which is not the case when solely information about the desired task is transferred through the channel, as was initially proposed in [13]. In that case, the operator would receive the force feedback from the previous task for a period equivalent to the round-trip time delay. Furthermore, in case the passivity of the channel is to be ensured for each task individually, additional caution would be required in order to compare the correct E in and E out of each task.
On the other hand, in cases where the bandwidth is limited, the transfer of augmented forces and velocities could lead to an increase in the time delay. In those cases, the switching solution proposed in [13] could be a preferable option.
It is also important to emphasize that the proposed approach allows for the use of multiple master devices with no delay between them, adding up to the total of n DoF, which can be used for commanding a higher number of tasks simultaneously. However, in case of delay between the master devices, the proposed theory has to be extended to allow multilateral teleoperation as in [40].

One-port Passivity of the Hierarchically-controlled Slave
As mentioned in Section 4.5, the fact that the PD controllers are implemented in F , but based on the error in V -coordinates, usually yields a non-passive closedloop system. This issue has been previously tackled in the literature (e. g., [18] and [19]) in order to allow passive interaction of the robot with the local environment. Nevertheless, in teleoperation, not only passive local interaction is desired, but also the passivity of the masterslave system. Therefore, additional issues arise, as will be described.
The block diagram representing the controlled slave, together with the environment, is depicted in Fig. 8. The block C represents the PD controllers applied for each task. The block B T is the mapping from decoupled task coordinates to original ones, as shown in Eq. 67. Moreover, the block S represents the slave dynamics, pre-compensated through Eq. 71, and E represents the environment dynamics. Finally, P C L is a local PC, responsible for passivating the port < F c , V s >, which will be described subsequently.
In [18] and [19] passivity of the whole-body controller is achieved by modifying the reference (or, conversely, the perceived error) given to the PD controller. To achieve that, an energy tank is connected to the port < V des , F c >, or conversely <q des , τ c >. However, in order to perform passive teleoperation with the null-space wall effect, a PC to ensure two-port passivity (i e., Eq. 40) has to be added to the port < V des , F c >, which corresponds to <V sd , F c > in the TDPA framework. Although the problem of energy regulation of multi-DoF systems usually allows for some redundancy (i. e., different velocities and forces can yield the same energetic behavior), adapting the value ofV sd for simultaneously ensuring the oneport passivity of <V sd , F c > and two-port passivity of the communication channel through <V sd , F c > would increase the complexity of the problem. This is due to the fact that the aforementioned velocity-force pairs and, consequently, their supplied energy (see Definition 1) are not the same. Therefore, the direction in whichV sd has to be adjusted in order to dissipate energy for both ports is not trivial (although possible to be found) and would demand a new formulation of the passivity controller.
An alternative solution for allowing passive interactions is to consider the projection term as part of the robot dynamics, instead of being part of the controller. In order to implement that idea, a local PC (P C L ) can be added to the system, as will be shown in Section 5.4.1.

Passivity of the Interaction Between Robot and Controller and One-port Passivity of the Slave Network
Passivity of the port < F c , V s > is a desired attribute for the system due to two main reasons. Firstly, as is the case in [18] and [19], it renders passive interactions of the slave controller with the local environment. In addition, as will be seen in Section 5.7, together with the passivity properties of the PD controller, it is also a sufficient condition to ensure one-port passivity of the slave-side network (see Fig. 4).
In order to achieve one-port passivity of the pair < F c , V s >, a local PC (P C L in Fig. 8), as presented in Section 3.2, can be applied. In this case, since the aforementioned port has admittance causality, F c will correspond to the input u while V s will correspond to the output y. Although the slave robot and the environment are continuous-time physical systems, it is assumed that Fig. 8 Slave network overview. C is the PD controller, B T is the mapping from decoupled task coordinates to original ones, S represents the pre-compensated slave dynamics, and E represents the environment dynamics the discrete-time port energy (see Eq. 20) can be used in the TDPA computations without jeopardizing the overall passivity of the system. Nevertheless, in case more than one task is being performed by the slave robot, additional care has to be taken in the choice of Γ (k). Although any positive definite Γ (k) would be able to render the system passive, its choice directly influences F pc , which will be introduced into Eq. 73 as with the subscript s added to the velocity V in order to cope with the TDPA notation, and F pc,i defined as where Γ i,j are the block elements of Γ , and V s,j is the original-task velocity of task j . It can be seen that the choice of Γ could create intertask influences, which are not desired. Furthermore, the intuitive choice of a block diagonal Γ , although maintaining the hierarchy, would not achieve decoupled dynamics in V s since the PC is applied as a damping force based on V s . Therefore, the following structure for Γ is proposed.
where Γ d (k) is a block diagonal matrix, composed of positive definite elements Γ d,i (k) (e. g., identity matrices or Λ i (q) as in [37]). The aforementioned choice of Γ (k) yields the following decoupled dynamics.
As it can be noted from Eq. 79, the proposed law implements a decoupled damper based on V s in order to passivate the port < F c , V s >.
It is important to note that, even though the dynamics is said to be decoupled, λ, and possibly Γ d , depends on the full configuration of the robot, q. Therefore, although bottomup disturbances do not directly occur, one task can, however, influence the others as it changes the configuration of the robot. This influence occurs in the form of extra damping added by the PC. Nevertheless, such a configuration dependence is also present in Λ(q) and μ(q,q). This is due to a similar reason for both the dynamic matrices and the TDPA ones, namely, they are derived from energy functions, which are affected by each subtask. While the former depend on the Lagrangian [31], the latter depend on W (k).
In case the coupling in λ and Γ d is not desired, the proposed approach could be transformed into its concatenated version [12], where each task would have a separate energy function, and the passivity of each task would be independently enforced.

Discrete-time Passivity of the PD Controller
It is well known that continuous-time PD controllers, as the one presented in Eq. 74, are passive systems [39]. However, the fact that the control action is usually computed in discrete time and applied to the robot actuators through a zero-order-holder (ZOH) can introduce extra energy into the system [15]. In order to avoid that, it is important that the lower bound on the damping of the controller, introduced in [15], is fulfilled.

Two-port Passivity of the Communication Channel
As mentioned in Section 3.3.1, passivity of the teleoperation system depends not only on the two-port passivity of the communication channel, but also on the one-port passivity of the master and slave networks. While one-port passivity of the master is naturally guaranteed, as will be shown in Section 5.7, one-port passivity of the slave port is enforced by the passivity of the PD controller and the local one-port PC introduced in Section 5.4.1. Therefore, the communication channel becomes the only source of energy introduction left to be passivated.
In order to passivate the communication channel, twoport TDPA can be applied. For that purpose, the approach presented in Section 3.3 can be applied using the augmented values of the velocities and forces, i. e., at the ports <F m , V m > on the master side and <V sd , F c > on the slave side. Nevertheless, additional care must also be taken in order to maintain the hierarchy of the tasks. Therefore, block-diagonal matrices Γ s (k) and Γ m (k) can be applied in the PC action, Eq. 44 and Eq. 48.
It is also important to remark that, if the task selection strategy presented in Section 5.3 is being applied, the PCs will only affect the commanded velocities and forces, which are non-zero. In case only one task is being commanded at a time, the PC will have its regular behavior, as for non-redundant robots. However, if more than one task is commanded at a time, the passivity controller will act based on the overall energetic behavior of the channel, but without violating the strictness of the task hierarchy.

Multi-DoF Drift Compensation
Despite being able to render the teleoperation system passive, TDPA introduces a well-known issue: a mismatch between the desired slave pose and the one given as reference to the slave controller. This issue was initially tackled for one DoF in [5,10,14] and later extended to Cartesian-space teleoperation of multi-DoF robots in [12]. In order to allow for multi-task teleoperation of redundant robots, an adaptation of the method proposed in [12] will be presented in this section, where the compensation law is computed for each task i and, then, concatenated as an augmented velocity. Since it comprises the full set of possible positions and rotations of a rigid body, the approach is derived on SE (3). For smaller-dimension tasks, special cases of the proposed approach can be easily derived.

Representation of Drift on SE (3)
For a task i comprising the complete Cartesian space, the velocitiesV sd,i (k) and V sd,i (k) can be defined as body velocities (see Section 2), represented in R 6 as whereD, i and D, i are the frames defined by the delayed master orientation and the orientation given to the slave as the reference for the given task, respectively. The discretetime integral ofD ,iV sd,i and D,i V sd,i can be computed following (13) as Using the definitions above, the drift present in task i at a given time step k can be represented in SE (3) by It can be noted from (47) and (81)-(83) that, if the slave PC acts at a time step, it will affect the value of g E,i for all future time steps. In case g E,i is not the identity matrix, there will be a drift between the delayed master pose and the pose given as reference to the slave for the given task.

Drift-compensation Law
In order to compensate for the drift caused by TDPA in each subtask, an additional velocity signal can be added to the delayed master velocity before it is checked by the PO. In Fig. 6 the drift compensator is represented by the block DC. By applying V ad before the point where the energies are computed, the modified velocityV sd (k) + V ad (k) will be checked and corrected for passivity. This guarantees that the compensation action will only be applied when so-called "passivity gaps" appear, i. e., when W S (k) > 0. Therefore, the compensator would not compromise the passivity of the system. From Fig. 6, it can been seen that, when the drift compensator is added, Eq. 47 for each task becomes where Ad E,i (k) is defined as [9] Ad E,i In order to reduce the drift between master and slave devices whenever allowed by the aforementioned passivity conditions, the following law can be used where , and the gains K T ,i ∈ R 3×3 and k R,i ∈ R are the translational and rotational gains of the compensator. Moreover, A(·) is defined in Section 2.

Convergence Analysis
As mentioned in Section 5.6.2, in order to keep the passivity, the proposed compensator is only able to reduce the drift when energy gaps are present. During the moments when the PC is acting to reduce the delayed master velocity coming from the channel, the accumulation of drift is unavoidable. For that reason, this section aims to analyze the convergence characteristics of the compensator during the moments where it is allowed to act, i. e. in the set where W S (k) > 0. At the moments where the compensation action is allowed, (85) becomes By defining a velocity error V E,i (k), (89) becomes From this definition, the error pose g E (k) can be defined as in (12) withD ,i V E (k) as its spatial velocity as follows Exploring the equality between the velocitiesD ,i V E,i and D,i V ad,i defined in (90), the error pose from (91) becomes It follows from the compensation law (87) and the definition of the exponential function on SE(3) (4) that the rotational part of (91) becomes which results in the following relation Likewise, exploring the identity from (9), the translational part becomes It can be verified that a sufficient condition for convergence is which ensures that as long as the trace of the accumulated rotational error, R E,i , is not equal to one, when the compensator is allowed to act after the drift has been accumulated by the PC. The above presented compensation law ensures that the magnitude of the drift is decreased from one time step to the next, even if the compensator is only allowed to act during a short period of time.

Particularities of the Multi-task Compensator
It is interesting to note that, as shown in Eq. 84, the drift compensator is applied as an augmented velocity. This means that, even if the master is not currently commanding a task, the compensator can still apply its action to that task, as long as there is an accumulated drift and a passivity gap.
An advantage of such a behavior is that the compensator may have the opportunity to compensate for the drift in a given task, even if not enough passivity gaps appeared during the time it was directly commanded. A disadvantage, however, is that a task that is not being directly commanded might move during the compensation action. If this behavior is not desired, an extra condition can be added to the compensation law, which only allows for the compensation ifV sd,i (k) = 0.

Overall Passivity
After having presented the whole-body teleoperation framework, this section aims at analyzing its passivity properties.
In order to conclude about the passivity of the overall teleoperation scheme, a set of assumptions about its composing elements are made.

Assumption 1
The gains of the slave PD controller are tuned according to [15], such that it remains passive for the sampling rate at which it operates.

Assumption 2
The human operator interacts passively with the master device.

Assumption 3 The environment is passive.
It is important to remark that such assumptions are commonly made in the TDPA literature (e. g. [3] and [11]) and do not pose significant obstacles to the application of the method.
Based on the aforementioned assumptions, the following lemmas can be stated, which will lead to the main result regarding the passivity of the overall system.

Lemma 2
When applying the pre-compensating control action Eq. 71, the port < F c − F ext , V s >, corresponding to the block S in Fig. 8, is passive w.r.t. Definition 1.
Proof Due to the power invariance property of the change of coordinates (see Eqs. 65 and 66), it suffices to prove that < F c − F ext , V s > is passive. For that purpose, we pick a storage function where Λ(q) is defined in Eq. 69. Using the decoupled dynamics after pre-compensation Eq. 73, noting that V s and V are the same velocity, and applying the passivity condition Eq. 53, the time derivative of W 1 can be written aṡ which shows that the port < F c − F ext , V s >, and consequently < F c − F ext , V s >, is passive according to Eqs. 18 and 19 with supplied energy W 1 .
Lemma 3 Based on Assumption 3 and Lemma 2, the port < F c , V s >, corresponding to the feedback interconnection of the blocks S and E in Fig. 8, is passive w.r.t. Definition 1.
Proof The result follows from the fact that the feedback interconnection of passive systems is passive, as stated in Corollary 1. Fig. 8, is passive w.r.t. Definition 1.

Lemma 4 The port < F c , V s >, corresponding to the series connection between the block B T and the feedback interconnection of the blocks S and E in
Proof Passivity is enforced by the application of the oneport passivity controller P C L , as described in Section 5.4.
Proof Passivity of the port is based on the fact that the continuous-time control law is passive (see [39]) and the discretization effects are compensating by having sufficiently high derivative gains, following [15].

Lemma 6
The slave-network port < V sd , F c >, corresponding to the dashed box S c in Fig. 8, is passive w.r.t. Definition 1 Proof Passivity follows from the fact that the aforementioned port consists in a feedback interconnection of the PD-controller port <Ṽ , F c > and the slave-robot port < F c , V s >, which according to Lemmas 4 and 5 are passive.

Lemma 7
Under Assumption 2, the interconnection between human operator and master device, corresponding to blocks H and M, respectively, in Fig. 7, is passive w.r.t. Definition 1.
Proof Since the master device is assumed to have dynamics similar to Eq. 52, its passivity can be proven by choosing the sum of its potential and kinetic energy as the storage function [24]. Therefore, the interaction port appears as the feedback interconnection of two passive systems, which, as shown in Corollary 1, is passive.

Lemma 8
The port < F m , V m > , corresponding to the dashed box M+H in Fig. 7, is passive w.r.t. Definition 1. where passivity of the port < F m , V m > follows from Lemma 7.

Lemma 9
The communication channel, together with the drift compensator, corresponding to blocks TDPN and DC, respectively in Fig. 6, is two-port passive.
Proof Passivity is enforced by two-port TDPA (blocks P C M and P C S in Fig. 6), as described in Section 5.5. Fig. 6, is passive.

Theorem 1 The overall teleoperation system, depicted in
Proof The result follows from Lemmas 6, 8, and 9.
It is important to remark that the passivity of the system does not depend on a particular choice of the selection matrix Ω. Therefore, even if multiple tasks are being commanded, which could result in a moving null-space wall, the system remains passive. Moreover, as previously stated, if multiple master devices are used, without delays between them, the passivity results still hold.

Validation Results
This section aims at presenting experimental results that validate the proposed approach.

Experimental Setup
The setup used to validate the proposed framework is shown in Fig. 9. The DLR force-feedback joystick (Fig. 9a) was chosen as the master device, used to capture the motion of the human operator's hand and provide force feedback according to the task being performed. The DLR joystick is a 2-DoF device, which has been previously used in the Kontur 2 mission for bilateral teleoperation from the International Space Station (ISS) [4].
Furthermore, the DLR Suspended Aerial Manipulator (SAM) [44] was chosen as the slave robot. It consists of an omnidirectional octarotor platform, which hangs from a carrier by means of cables. Attached to the platform is a 7-DoF torque-controlled manipulator. It is important to mention that, since the proposed architecture is based on computed control forces only, the force-torque sensor attached to the end-effector was not used.
For the purpose of this paper, the SAM was operated indoors, attached to the ceiling through an approximately 3-meter-long lightweight cable system, whose length was kept constant throughout the task. Due the aforementioned characteristics of the cable system, it was assumed that no motion other than rotation around its suspension axis (yaw) was performed by the platform. Therefore, the overall aerial manipulator was assumed to be an 8-DoF robot. The master and slave robots exchanged data through a wireless communication network, which introduced an average round-trip time x of 30mm, in addition to package loss and jitter. Adding to that, additional virtual delay of 100ms was added in order to simulate a more realistic use case outside of the well-controlled lab environment.
As can be seen in Fig. 9, in addition to haptic feedback, the only source of visual information provided to the operator were time-delayed images from the camera attached to the platform, which were streamed to the monitor in front of him. An example of the view provided to the operator can be seen in Fig. 9c. The same image was also shown on the TV behind the operator, such that Fig. 9a would provide a complete overview of the entire scenario. Despite the proximity between human and robot, no eye contact was allowed. Moreover, due to the reduced number of DoF of the master device compared to the slave robot, the operator was allowed to change the desired tasks to be commanded according to the selection strategy presented in Section 5.3 by manually introducing the task number through a user interface.

Task Description
The task chosen to validate the framework was picking and placing of a cage, which has also been used in the scope of the AEROARMS project [36] to deploy an inspection robot (see [28]).
The task was composed of five stages: 1. Commanding task 2 (platform orientation) in order to align the camera image with the action frames, i. e., such that moving left/right, forward/backward on the joystick would mean the same in the image. 2. Commanding task 1 (end-effector pose) to steer the hook to the surface of the cage. 3. Commanding task 1 to slide the hook on the surface of the cage until it goes into the handle and lift it. 4. Commanding task 2 to change the camera view in order to ensure that the cage is being lifted. 5. Commanding task 1 to place the cage back down.

Results
Figures 10, 11, 12 and 13 depict the results of the application of the proposed framework in order to fulfill the pick-andplace task. Figure 10 shows commanded (master) and actual (slave) position of the end-effector in an inertial frame. Figure 11 shows commanded and actual yaw angle of the platform (top), the total angular error between desired and actual orientation of the end-effector (middle), and the norm of the drift compensator velocity V ad . Figure 12 depicts the norms of the master and slave end-effector forces (top) and torques (middle) as well as torques of the platform around its yaw axis (bottom). It is important to remark that the norm of the master forces/torques had its sign inverted in order to remind the reader that the sign of the haptic force applied by the master device is the opposite of the one applied by the slave-side controller. Lastly, Fig. 13 shows the energies

Multi-task Teleoperation
In Figs. 10 and 11, it can be noted that, initially, the human operator commands the robot to keep its initial pose. Then, after around 30s the base (task 2) is commanded to move in the null space of the end-effector, as shown in the top plot of  Euclidean norm and negative of the Euclidean norm of master and slave, respectively, end-effector forces and torques, and platform master and slave torque around the its suspension axis Fig. 11. Due to the strict hierarchy imposed by the wholebody controller, even though the base changes its orientation in about 0.5rad (approx. 28.6 • ), the pose of the end-effector only slightly deviates from its desired value. That small deviation owes to the imperfect knowledge of the system dynamics and imperfect gravity compensation, which also causes steady state error in the manipulator orientation (see Fig. 10 -middle plot). After achieving a desired view, the operator switches to task 1 and, by choosing two axes at a time to be commanded, ensures that the hook is finally introduced into the handle. Subsequently, the cage is lifted (see z position). It can be seen in Fig. 10 and in the middle plot of Fig. 11 that, during the teleoperation of the arm, the commanded and actual poses deviate significantly at given moments. This is due to the interaction with the environment, which not only includes the lifting phase, but also touching the cage and sliding onto it, which allows the operator to better understand the location of the hook through haptic feedback since the camera images do not provide depth information. In those moments, it can be seen in Fig. 12 that the forces and torques applied to the end-effector and fed back to the Fig. 14 Sequence of screenshots from the experiment video master increase in magnitude as usual for contacts with the environment. Finally, after lifting the cage, the operator commands a change in orientation to the platform (at around 180s) in order to ensure that the cage is being lifted. Lastly, the cage is placed back down.

Null-space Wall
Another interesting behavior can be noted in Fig. 11 top and Fig. 12 -bottom. As being a secondary task, the base controller only ensures that the commands of the operator are followed in case it can do so without disturbing the main task. As can be seen in the aforementioned figures, the secondary task is almost constantly fulfilled. However, shortly before 150s the primary task controller requires that the flying platform also moves in order to keep the pre-defined hierarchy. Therefore, the secondary controller increases its torque command as the deviation increases. Since the base orientation is a secondary task, that torque is not commanded to the actuators, but rather projected to zero. However, in the proposed framework, it is fed back to the operator, who would feel as if a hard wall would hit the robot if the secondary task is selected as active in the task-selection strategy. This behavior shows the null-space wall concept, described in Section 5.1.

Passive Teleoperation with Drift Compensation
The behavior of the three proposed passivity controllers can be assessed in Fig. 13. The blue dashed lines represent the in energies while the red solid lines and green dashdotted ones represent the out and PC energies, respectively. The top plot shows the energetic behavior on the slave-side of the communication channel. It can be seen that, even though the slave-side PC removed less energy compared to the master-side one, its action was necessary to ensure that E S out (k) ≤ E M in (k − k b ). In addition, it can be seen in Fig. 11 -bottom that, during the moments when the slave-side PC was activated, it caused the slave reference to drift, which yielded non-zero values of V ad . However, as passivity gaps appeared, the drift compensator was capable of successfully eliminating the drift.
Moreover, it can seen in the middle plot, that the masterside PC had to be more frequently activated in order to keep the channel passive. As en effect, high frequency oscillations could be noticed in the master-device forces in Fig. 12. Although being noticeable to the human operator, the sudden force changes did not prevent the successful completion of the task. Nevertheless, in applications where that behavior is undesirable, a passive filter can be applied in order to smoothen the force signal while maintaining the passivity of the system (see [42]).
Ultimately, it can be seen that the local passivity controller only had to remove a small amount of energy during the initial motion of the platform. However, if it were deactivated, the passivity of the system would be jeopardized, and stability would not be guaranteed anymore. Adding to that, for different tasks and controller gains, the local whole-body controller can introduce higher amounts of out energy into the system making it more frequently active (see [18] and [19]).

Supplementary Video
In order to further facilitate the reader's understanding of the approach, a supplementary video can be found at https:// youtu.be/ mXeeqcUHws. There, the sequence depicted in Fig. 14 is performed. Figure 14a and b show the initial pose of the robot, and the camera view, respectively. Figure 14c and d show the system after the base was commanded to move in the null space of the end-effector in order to align the arm motion with the camera frame. Finally, Fig. 14e and f, and g and h show the pick and place maneuvers, respectively.

Conclusions and Future Work
This paper presented a novel passivity-based framework to allow for the time-delayed hierarchical whole-body bilateral teleoperation of redundant robots. Within the proposed framework, a human operator is allowed to command different tasks of a remote slave robot through one or more master devices while receiving meaningful haptic feedback and being able to online switch among tasks while the other tasks are controlled autonomously.
The approach was experimentally validated using the SAM as the slave robot and the DLR force-feedback joystick as the master device. Results showed that the proposed framework allowed the aforementioned aerial manipulator to be teleoperated in a hierarchically-decoupled manner in order to not only fulfill an end-effector task, but also provide the operator with the capability of steering the flying base in order to achieve a desired view from the camera attached to it. Future work will involve adapting our framework such that, in addition to having some tasks being teleoperated and others being performed autonomously, the authority of a single task can be simultaneously shared between human and machine (e. g. based on the quality of the measurements provided by the on-board sensors [7]). In addition the oscillation control method presented in [43] will be incorporated to the system in order to allow it to be applied to more complex scenarios, where the oscillation of the suspended system cannot be neglected.