Generalized Framework for Control of Redundant ...

Sandoval J.*, Vieyres P., Poisson G.

Generalized Framework for Control of Redundant Manipulators in Robot-Assisted Minimally Invasive Surgery 

Abstract— In this paper, we provide a generalized framework for the dynamic control of redundant manipulators used for robotassisted minimally invasive surgeries (RA-MIS). During a RA-MIS, a robot inserts a surgical tool into the patient’s body through a surgical device placed at the incision position, known as the trocar. A kinematic constraint is then generated since the tool axis must always pass through the trocar point, i.e. the Remote Center of Motion (RCM) constraint, while the tool-tip executes the surgical task. When a serial manipulator is used, the RCM constraint must be guaranteed by the control system. Moreover, we consider the event of desired or unexpected collisions between the robot’s body and its environment, e.g. medical staff or operating room equipment. In order to guarantee the accomplishment of the surgical task in the event of collisions, we propose a joint compliance strategy, by exploiting the Jacobian null-space. The control framework proposed in this paper deals simultaneously with the surgical tool-tip trajectory, the RCM constraint and collisions in the robot’s body. Simulations were conducted to validate the effectiveness of the proposed formulation, using a Kuka LBR 7 iiwa R800 robot arm. Keywords: Robot-assisted Minimally Invasive Surgery, RCM constraint, redundant robot, torque-control

1. INTRODUCTION In robot-assisted Minimally Invasive Surgery (RA-MIS), a surgeon tele-operates one or more robots inserting surgical tools into the patient’s body through surgical devices, i.e. trocar, placed on each small incision cut. Besides the benefits given by the MIS compared to classical open surgery [1], e.g. reduction in recovery time, low risk of infection or minimization of scars, the robot-assisted system increases the workspace of the surgeon, usually considerably constrained in classical MIS and enables to cancel his trembling and to enhance accuracy, among other advantages. During a RA-MIS, such as in gynecology, urology or general surgery, the robot tool-tip performs a task inside the patient’s body. Simultaneously, the tool movements are constrained by the trocar where the contact forces must be minimized, in order to avoid any injury to the patient. The kinematic constraint generated by the incision point is commonly known as the Remote Center of Motion (RCM) constraint. Several “RCM mechanisms” have been specially designed to mechanically define a RCM point and then to calibrate it with the trocar [2, 3]. Some commercialized robot-assisted MIS, such as the Da Vinci surgical system, includes a fixed RCM point for each robotic arm [4]. Before starting the surgical procedure, the RCM point is synchronized with the trocar position, in such a way that the surgical tool always goes through the trocar. Then, the surgical robot can safely perform an autonomous or tele-operated task into the patient’s body. However, no specific strategy is planned to prevent a degradation of the surgical task or a physical interference on the robotic system when collisions between the robotic arms and the environment occur. Instead of considering a robot with a mechanically fixed RCM point, a serial manipulator can also be used for RA-MIS, where the RCM constraint must be guaranteed by a software control approach, e.g. MIRO robot from DLR [5]. The use of serial manipulators gives versatility to the system, since the robot is not restricted to be used in applications with a fixed incision point. Moreover, when the robot is kinematically redundant, other secondary tasks can eventually be performed. Different control approaches have been proposed to effectively combine the surgical task and the RCM constraint using serial redundant manipulators. For instance, Aghakhani [6] provided a general kinematic characterization of the RCM constraint for MIS. HyoJeong [7] proposed to minimize the constraint force at the RCM by considering the manipulator as a closed chain; however, a non-redundancy property of the manipulator is required. Michelin [8] proposed the application of the conventional operational space formulation, developed by Khatib [9, 10], by choosing the objective function such as the square of the minimal distance between the insertion position and the surgical tool axis. Additionally, in this las work, a trajectory-following control is applied to the end-effector’s motion. In a previous study, we improved the null-motion performance of the control system proposed in [8], by implementing an extended-based formulation with null-motion feedback [11]. However, the control approaches proposed in [8] and [11] are performed at the acceleration level and, consequently, are highly dependent of an accurate information or estimation of the robot inertia matrix. The null-space of the robot’s Jacobian can be exploited to perform other secondary tasks, improving the performances of the surgical procedure. Redundancy can be conveniently used to execute obstacles avoidance strategies [12], to optimize manipulability [13], to minimize gravity torques [14] or apparent mass and frictions [15]. If we consider that the robot shares a common workspace with the medical staff and the operating room equipments, collisions with the robot’s body may occur during the surgical procedure, degrading the performances of the surgical task. In this context, a contact might happen due to multiple reasons: accidental collisions between the robot and the environment, e.g. staff or medical equipments, desired contacts by the medical staff to modify the robot joint configuration, and so on. Therefore, an interesting way to exploit the redundancy of the robot is to implement a compliant motion strategy in the robot’s body, in order to preserve the surgical task performances in the

event of collisions. Numeous compliance strategies applied in the null-space of the robot can be found in the literature. For instance, Sadeghian [16] proposed a null-space compliance strategy at the acceleration level using observers to estimate the external torques. A multi-priority impedance controller without using external forces sensor at the end-effector was proposed in [17], where the first level of priority is given to the Cartesian impedance control, whereas the joint impedance control is performed in a secondary level. Nevertheless, the control approaches proposed in [16] and [17] are highly dependent on the accuracy of the estimated inertia matrix. A simplified compliance control approach is proposed in [18], using the potential function of a virtual spring and adding an appropriate damping term, to avoid to shape a desired inertia matrix and without using the estimated/measured external forces. In this paper, a generalized torque-control framework for RA-MIS is proposed using kinematically redundant manipulators. The proposed framework allows to simultaneously integrate the surgical task performed by the tool-tip while guaranteeing the RCM constraint as well as dealing with collisions with the robot’s body. A strict hierarchy redundancy resolution method is applied [19] in order to define the priority order of these different tasks. In this strict hierarchy approach, a higher priority task is never disturbed by a lower priority task. In the proposed control framework, we define the surgical task performed by the tooltip in the first level of priority, by implementing a Cartesian compliance control approach. A kinematic formulation of the RCM constraint, that we previously proposed in [20], is exploited in the second level of priority, in order to constraint the tool movements by the trocar position. Finally, the third level of priority is exploited to implement a joint compliance strategy dealing with collisions with the robot’s body (Figure 1).

Figure 1. Hierarchical tasks order in the torque-control approach

The paper is organized as follows. Section II presents the generalized framework approach and a detailed definition of each task. Section III reports the simulation results using the dynamic model of a Kuka LBR 7 iiwa R800 robot arm. A discussion of the obtained results is also proposed in this section. The last section is devoted to conclusions of the proposed control framework. 2.

MATERIAL AND METHODS

2.1. Strict Priority Control Approach When using a redundant robot to perform a minimally invasive surgical task, a tool is inserted by the robot inside the patient’s body through a trocar device affixed at the surgical incision. The surgical task performed by the robot’s tool-tip is generally tele-operated by the surgeon through a master device, so the robot reproduces the gestures performed by the surgeon with the master device. This latter can be a haptic device, able to provide a force feedback to the surgeon. Moreover, the movements of the surgical tool are then restricted by the kinematic constraint generated by the trocar, i.e. RCM constraint. Figure 2 shows a minimally invasive surgical scene representation, with the forces applied to the robot’s body in the event of collisions. In order to simultaneously perform the surgical task, control the RCM constraint and manage the possible collisions with the robot’s body, we introduce a new torque control framework for redundant manipulators, using the strict hierarchy redundancy resolution proposed in [21] and successfully implemented in [22] [23]. Firstly, the dynamic model of a 𝑛-DOF serial manipulator can be written in joint-space coordinates as follows:

𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) − 𝜏𝑒𝑥𝑡 = 𝜏𝑐

(1)

where 𝑀(𝑞) ∈ ℜ𝑛×𝑛 represents the inertia matrix, 𝐶(𝑞, 𝑞̇ )𝑞̇ ∈ ℜ𝑛 is the generalized vector of centrifugal and Coriolis forces, 𝑔(𝑞) ∈ ℜ𝑛 is the generalized vector of gravitational forces, 𝜏𝑐 ∈ ℜ𝑛 and 𝜏𝑒𝑥𝑡 ∈ ℜ𝑛 are the generalized control and external torques, respectively, and 𝑞 ∈ ℜ𝑛 is a vector of generalized joint coordinates. A definition for the control torque 𝜏𝑐 ∈ ℜ𝑛 with compensation of dynamics effects, i.e. 𝐶̂ (𝑞, 𝑞̇ ) ≈ 𝐶(𝑞, 𝑞̇ ) and 𝑔̂(𝑞) ≈ 𝑔(𝑞), is given by: 𝜏𝑐 = 𝜏 + 𝐶̂ (𝑞, 𝑞̇ )𝑞̇ + 𝑔̂(𝑞)

(2)

Where the torque 𝜏 ∈ ℜ𝑛 represents a set of 𝑟 number of control torque tasks (𝜏1 , 𝜏2 , …, 𝜏𝑟 ) organized hierarchically, e.g. 𝜏1 has a higher priority than 𝜏2 , this latter is of a higher priority than 𝜏3 and so on. The dimension of each task 𝜏𝑖 ∈ ℜ𝑛 is 𝑚𝑖 , for 𝑖 = 1, … , 𝑟. The torque 𝜏 is then defined by using null-space projectors to guarantee that a task is not perturbed by another lower priority task, as follows: 𝜏 = 𝜏1 + ∑𝑟𝑖=2 𝑁𝑖 (𝑞)𝜏𝑖 = 𝐽1𝑇 𝐹1 + ∑𝑟𝑖=2 𝑁𝑖 (𝑞)𝐽𝑖𝑇 𝐹𝑖

(3)

Where the null-space projector 𝑁𝑖 (𝑞) ∈ ℜ𝑛×𝑛 is defined by: 𝐴 𝐴 𝑁𝑖 (𝑞) = 𝐼 − 𝐽𝑖−1 (𝑞)𝑇 (𝐽𝑖−1 (𝑞)+ )𝑇

(4)

𝐴 𝐴 (𝑞), The matrix 𝐽𝑖−1 (𝑞)+ ∈ ℜ𝑛×𝑚𝑡 is the well-known inertial-weighted generalized inverse of 𝐽𝑖−1 proposed in [9], where 𝑘 𝐴 𝑚 ×𝑛 𝑚𝑡 = ∑𝑖=1 𝑚𝑖 . The augmented Jacobian matrix 𝐽𝑖−1 (𝑞) ∈ ℜ 𝑡 can be defined as follows:

𝐽1 (𝑞) 𝐽 (𝑞) 𝐴 (𝑞) ] 𝐽𝑖−1 =[ 2 ⋮ 𝐽𝑖−1 (𝑞)

(5)

As mentioned before, in the context of the RA-MIS using a torque-controlled redundant manipulator, the strict hierarchy in (3) can be used to execute some important tasks included in the surgical procedure. In the following, we study in detail the implementation of the three identified tasks. In our case, 𝐽1 (𝑞) ∈ ℜ𝑚1×𝑛 corresponds to the Jacobian matrix mapping the joint velocities 𝑞̇ to the Cartesian velocities 𝑥̇ ∈ ℜ𝑚1 of the tool-tip, i.e. 𝑚1 = 3, whereas 𝐽2 (𝑞) = 𝐽𝑅𝐶𝑀 (𝑞) is the Jacobian matrix performing a mapping between the joint velocities 𝑞̇ and the RCM constraint coordinates, as explained below.

Figure 2. Representation of a complete tele-operated serial robot-assisted MIS system. The robot tool-tip is tele-operated to perform a surgical task, while a RCM constraint is guaranteed at the incision point

2.2. Cartesian Compliance Control In order to perform the minimally invasive surgical task, a desired tool-tip trajectory 𝑥𝑑 (𝑡) ∈ ℜ3 is imposed by the surgeon, for instance by using the master device. We propose to use a compliance control strategy based on the definition of the potential function of a virtual spring and added to a damping term, as used in [18], given by: 𝜏1 = 𝐽1𝑇 𝐹1 = 𝐽1𝑇 ((

𝜕𝑃(𝑥𝑒 ) 𝑇 𝜕𝑥

) + 𝐷𝑥 (𝑥̇ 𝑑 − 𝑥̇ ))

(6)

where 𝐷𝑥 ∈ ℜ3×3 is a positive definite diagonal matrix corresponding to the damping parameter. Moreover, the virtual potential function 𝑃(𝑥𝑒 ) is defined as a function of the difference between the desired and the actual Cartesian trajectory 𝑥𝑒 (𝑞) = 𝑥𝑑 − 𝑥, as follows: 1

𝑃(𝑥𝑒 (𝑞)) = 2 𝑥𝑒 (𝑞)𝑇 𝐾𝑥 𝑥𝑒 (𝑞)

(7)

Defining 𝐾𝑥 ∈ ℜ3×3 as a positive definite diagonal matrix corresponding to the stiffness parameter in the compliance law. The control torque 𝜏1 can then be redefined as: 𝜏1 = 𝐽1𝑇 [𝐾𝑥 (𝑥𝑑 − 𝑥) + 𝐷𝑥 (𝑥̇ 𝑑 − 𝑥̇ )] Equation (6) implements a Cartesian compliance control, regulated by the gains 𝐾𝑥 and 𝐷𝑥 . 2.3. Remote Center of Motion Constraint Control torque 𝜏2 corresponding to the second priority-level task guaranteeing the RCM constraint, can be written as:

(8)

𝑇 𝑇 [𝐾 (𝑡 𝜏2 = 𝑁2 𝐽𝑅𝐶𝑀 𝐹2 = 𝑁2 𝐽𝑅𝐶𝑀 𝑅 𝑑 − 𝑡) + 𝐷𝑅 (𝑡̇𝑑 − 𝑡̇ )]

(9)

where the task 𝑡 ∈ ℜ1 corresponds to the minimal distance 𝐷 between the trocar position 𝑃𝑡𝑟𝑜𝑐𝑎𝑟 ∈ ℜ3 and the surgical tool axis (Figure 3). Therefore, in order to guarantee the RCM constraint, the desired task value is set as 𝑡𝑑 = 0 𝑎𝑛𝑑 𝑡̇𝑑 = 0. The Jacobian matrix 𝐽𝑅𝐶𝑀 (𝑞) ∈ ℜ1×𝑛 is defined as follows: ̂ 𝑇 (𝜕𝑃𝑅𝐶𝑀 (𝑞)) 𝐽𝑅𝐶𝑀 (𝑞) = 𝐷 (10) 𝜕𝑞

̂ is the unit vector in the direction from 𝑃𝑅𝐶𝑀 toward 𝑃𝑡𝑟𝑜𝑐𝑎𝑟 . Moreover, 𝑃𝑅𝐶𝑀 (𝑞) ∈ ℜ3 is the potential coinciding where 𝐷 𝑇 point at the link 𝑖 (tool) with the trocar: 𝑃𝑅𝐶𝑀 (𝑞) = 𝑃𝑖 (𝑞) + 𝑙̂𝑖 (𝑃𝑡𝑟𝑜𝑐𝑎𝑟 − 𝑃𝑖 )𝑙̂𝑖 . The unit vector 𝑙̂𝑖 can be easily defined as 𝑙̂𝑖 = (𝑃𝑖+1 − 𝑃𝑖 )⁄𝑙𝑖 . The detailed method to define the Jacobian matrix 𝐽𝑅𝐶𝑀 (𝑞) ∈ ℜ1×𝑛 can be found in [20]. Similar to Eq. (6), the force 𝐹2 in the Eq. (9) is used to apply a compliance control at the RCM coordinates, regulated by the gains 𝐾𝑅 ∈ ℜ1 and 𝐷𝑅 ∈ ℜ1.

Figure 3. Geometrical description of the RCM constraint where the distance 𝐷 must be minimized

2.4. Joint Compliance Control A third priority-level task will be applied, consisting in the implementation of a joint compliance control allowing to deal with collisions with the robot’s body, as follows: 𝜏3 = 𝑁3(𝐾𝐽 (𝑞𝑑 − 𝑞) − 𝐷𝐽 𝑞̇ )

(11)

̂ in order to where, 𝑞𝑑 ∈ ℜ𝑛 is a desired robot joint configuration and the damping gain 𝐷𝑗 ∈ ℜ𝑛×𝑛is defined by 𝐷𝑗 = 𝑑𝑗 𝑀 𝑛×𝑛 stabilize the internal motion, as suggested in [9]. The stiffness gain 𝐾𝐽 ∈ ℜ imposes a compliance behavior when collision occurs. Once the contact disappears, the desired joint configuration is recovered. The overall control framework is schematized by the block diagram represented in the Figure 4.

Figure 4. Block diagram representing the proposed control architecture. The control torque 𝜏𝐶 is composed of the centrifugal, Coriolis and gravitational compensation torques, the measured external torques 𝜏̂𝐸𝑋𝑇 as well as the control torques 𝜏𝑇 and 𝜏𝑁 related to the surgical task and the null-space compliance task, respectively

3. RESULTS AND DISCUSSION In order to validate the control framework proposed in this paper, the dynamic model of the Kuka LBR 7 iiwa R800 robot arm was used. This robot is a redundant torque-controlled one with 7 rotational joints. In the proposed study (Figure 5), the robot holds a surgical tool at the end effector and inserts it through a trocar, with position 𝑃𝑡𝑟𝑜𝑐𝑎𝑟 = [0,0.5,0.1] m. The desired tool-tip trajectory is defined between the incision point and a target point 𝑇𝑎𝑟𝑔𝑒𝑡 = [0,0.55,0.05] m.

Figure 5. Study case using dynamic model of the Kuka LBR 7 iiwa R800 robot arm. A desired linear trajectory is given to the tool-tip while guaranteeing the RCM constraint. Moreover, a collision occurs with joint 3 between 𝑡𝑖𝑚𝑒 = 3 s and 𝑡𝑖𝑚𝑒 = 4 s

The desired stiffness and damping parameters for the Cartesian compliance strategy were adjusted as follows: 𝐾𝑥 = 𝑑𝑖𝑎𝑔[700,700,700] N/m and 𝐷𝑥 = 𝑑𝑖𝑎𝑔[70,70,70] Ns/m, respectively. Concerning the secondary task preserving the RCM constraint, the parameters values were set as: 𝐾𝑅 = 500 N/m and 𝐷𝑅 = 50 Ns/m. Finally, the compliance parameters for the joint compliance task were fixed at: 𝐾𝑗 = 𝑑𝑖𝑎𝑔[100,100,100,100,100,100,100] Nm/rad and 𝑑𝑗 = 30 s-1. At time = 0 s, the tool-tip follows a desired linear trajectory; during a time interval chosen between 3 s and 4 s, a collision occurs between the third robot’s link and a compliant environment of stiffness value 𝑘𝑒 = 30 Nm/rad. Before the collision starts, the joint compliant task is not activated, i.e. 𝑞𝑑 = 𝑞. Once a stabilized joint configuration is achieved by the robot in time = 2.7 s, the joint configuration task is then activated by fixing the desired configuration as 𝑞𝑑 = 𝑞(𝑡𝑖𝑚𝑒 = 2.7 s). On this way, the stiffness effect of the joint compliance task is activated, allowing to preserve as best as possible the surgical task during the collision. Likewise, the initial joint configuration was set to 𝑞(0) = [15.2,31.9,50.5,115.9, −127.3,49,0] deg. We verify the performances of the three tasks, i.e. the tool-tip trajectory accuracy, the position error (distance D) at the RCM constraint as well as the compliant effect provided by the joint compliance task. The minimization of the distance 𝐷 between the trocar position 𝑃𝑡𝑟𝑜𝑐𝑎𝑟 and the surgical tool axis is shown in Figure 6. The control approach effectively guarantee minimal values for the distance, preserving the RCM constraint based on the position of the trocar device. It is evidenced that the largest errors take place during the collision between time = 3 s and time = 4 s. Nevertheless, the distance 𝐷 never increases more than 0.5 mm. These error values have a similar order of magnitude compared with those found in previous studies [1] [11] [24]. It is worth mentioning that these errors also depend on the compliance parameters set in the control task.

Figure 6. Minimization of the distance 𝐷 between the trocar position and the surgical tool axis

Figure 7 shows the tool-tip trajectory, where it is evidenced that the robot’s tool-tip effectively follows the imposed desired trajectory. A slight deviation of the desired trajectory can be observed during the collision period, caused by the dynamic effects generated by the unknown external force applied to the robot’s body. At the end of the collision, the Cartesian compliance strategy is able to quickly recover the same Cartesian position obtained before the collision. Moreover, the joint position trajectories are presented in Figure 8, where it is evidenced the effectiveness of the joint compliance task. Before the collision, a joint configuration is almost stabilized. Then, the joint positions are modified by the impact of the collision. Once the collision time is finished, the joint compliance strategy allows to reconfigure the robot joint positions of the robot and recover the stabilized configuration after the collision. Figures 6-8 also evidence an optimal independent performance of each task, where the compliance behavior is confirmed for each priority level. It is important to mention that we imposed a fixed desired trajectory to the tool-tip, in order to validate the control framework. However, in the medical context, the desired trajectory is expected to be generated online by the surgeon with a master device.

In the proposed experimentation, we utilized a 7-DOF redundant manipulator, but the control approach can also be applied to other redundant robots with more degrees of freedom. Moreover, the increase of DOF generally improves the task performances because it provides a largest available null-space to execute the secondary tasks.

Figure 7. X, Y and Z task-space trajectories for the surgical tool-tip

Figure 8. Joint position trajectories for the studied case

4. CONCLUSION This paper proposes a generalized control framework for robot-assisted minimally invasive general surgery, using torquecontrolled redundant manipulators. During a surgical task procedure, besides the accomplishment of the desired surgical task performed by the tool-tip, the proposed control system also guarantees that the surgical tool always goes through the trocar device placed at the incision position of the patient’s body, preserving the generated RCM constraint. The framework uses a strict priority method based on null-space projectors, allowing to simultaneously integrate multiple tasks organized by a priority order. In the case of RA-MIS, we proposed the implementation of three main tasks listed here from higher to lower priority level: the tool-tip trajectory, the RCM constraint and joint compliance control in case of collisions in the robot’s body. A compliance law was applied for each control task. The dynamic model of a torque-controlled robot was used to validate the effectiveness of the framework. The results showed the capacity of the framework to simultaneously comply the three tasks, always respecting the priority order between the tasks. 5. ACKNOWLEDGMENT This research was supported by the French Ministry of Research. 6. REFERENCES [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] [24]

H. Azimian, R. V. Patel, and M. D. Naish, "On constrained manipulation in robotics-assisted minimally invasive surgery," in Biomedical Robotics and Biomechatronics (BioRob), 2010 3rd IEEE RAS and EMBS International Conference on, 2010, pp. 650-655. C.-H. Kuo and J. Dai, "Robotics for Minimally Invasive Surgery: A Historical Review from the Perspective of Kinematics," in International Symposium on History of Machines and Mechanisms, H.-S. Yan and M. Ceccarelli, Eds., ed: Springer Netherlands, 2009, pp. 337-354. C. H. Kuo, J. S. Dai, and P. Dasgupta, "Kinematic design considerations for minimally invasive surgical robots: an overview," Int J Med Robot, vol. 8, pp. 127-45, Jun 2012. C. Freschi, V. Ferrari, F. Melfi, M. Ferrari, F. Mosca, and A. Cuschieri, "Technical review of the da Vinci surgical telemanipulator," The International Journal of Medical Robotics and Computer Assisted Surgery, vol. 9, pp. 396-406, 2013. U. Hagn et al., “The DLR MIRO: a versatile lightweight robot for surgical applications,” Industrial Robot: An International Journal, vol. 35, no. 4, pp. 324–336, Jun. 2008. N. Aghakhani, M. Geravand, N. Shahriari, M. Vendittelli, and G. Oriolo, "Task control with remote center of motion constraint for minimally invasive robotic surgery," in Robotics and Automation (ICRA), 2013 IEEE International Conference on, 2013, pp. 5807-5812. C. Hyo-Jeong and Y. Byung-Ju, "Modeling of a constraint force at RCM point in a needle insertion task," in Mechatronics and Automation (ICMA), 2011 International Conference on, 2011, pp. 2177-2182. M. Michelin, P. Poignet, and E. Dombre, "Dynamic task/posture decoupling for minimally invasive surgery motions: simulation r esults," in Intelligent Robots and Systems, 2004. (IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference on, 2004, pp. 3625-3630 vol.4. O. Khatib, "A unified approach for motion and force control of robot manipulators: The operational space formulation," Roboti cs and Automation, IEEE Journal of, vol. 3, pp. 43-53, 1987. O. Khatib, "Motion/force redundancy of manipulators," in Japan—USA Symposium on Flexible Automation, Kyoto, Japan, July, 1990, pp. 337-342. J. Sandoval, G. Poisson, and P. Vieyres, "Improved dynamic formulation for decoupled cartesian admittance control and RCM constraint," in 2016 IEEE International Conference on Robotics and Automation (ICRA), 2016, pp. 1124-1129. A. A. Maciejewski and C. A. Klein, “Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments,” The international journal of robotics research, vol. 4, no. 3, pp. 109–117, 1985. T. Yoshikawa, “Manipulability and redundancy control of robotic mechanisms,” in Robotics and Automation. Proceedings. 1985 IEEE International Conference on, 1985, vol. 2, pp. 1004–1009. A. Ajoudani, M. Gabiccini, N. G. Tsagarakis, and A. Bicchi, “Human-like impedance and minimum effort control for natural and efficient manipulation,” in Robotics and Automation (ICRA), 2013 IEEE International Conference on, 2013, pp. 4499–4505. J. G. Petersen, S. A. Bowyer, and F. R. y Baena, “Mass and friction optimization for natural motion in hands-on robotic surgery,” IEEE Transactions on Robotics, vol. 32, no. 1, pp. 201–213, 2016. H. Sadeghian, L. Villani, M. Keshmiri, and B. Siciliano, “Task-space control of robot manipulators with null-space compliance,” IEEE Transactions on Robotics, vol. 30, no. 2, pp. 493–506, 2014. R. Platt, M. Abdallah, and C. Wampler, “Multiple-priority impedance control,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on, 2011, pp. 6033–6038. A. Dietrich, T. Wimbock, A. Albu-Schaffer, and G. Hirzinger, “Integration of reactive, torque-based self-collision avoidance into a task hierarchy,” IEEE Transactions on Robotics, vol. 28, no. 6, pp. 1278–1293, 2012. A. Dietrich, C. Ott, and A. Albu-Schäffer, "An overview of null space projections for redundant, torque-controlled robots," The International Journal of Robotics Research, p. 0278364914566516, 2015. J. Sandoval, G. Poisson and P. Vieyres, "A New Kinematic Formulation of the RCM Constraint for Redundant Torque-Controlled Robots" in Intelligent Robots and Systems, 2017. (IROS 2017). Proceedings. 2017 IEEE/RSJ International Conference on, 2017. Accepted. B. Siciliano and J. J. E. Slotine, "A general framework for managing multiple tasks in highly redundant robotic systems," in Advanced Robotics, 1991. 'Robots in Unstructured Environments', 91 ICAR., Fifth International Conference on, 1991, pp. 1211-1216 vol.2. H. Sadeghian, L. Villani, M. Keshmiri, and B. Siciliano, "Dynamic multi-priority control in redundant robotic systems," Robotica, vol. 31, pp. 11551167, 2013/005/22 2013. N. Mansard, O. Khatib, and A. Kheddar, "A unified approach to integrate unilateral constraints in the stack of tasks," IEEE Transactions on Robotics, vol. 25, pp. 670-685, 2009. R. C. Locke and R. V. Patel, “Optimal remote center-of-motion location for robotics-assisted minimally-invasive surgery,” in Robotics and Automation, 2007 IEEE International Conference on, 2007, pp. 1900–1905.

*

(Corresponding author) Juan Sandoval is a third year Ph.D. student on Robotics at PRISME Laboratory, University of Orléans, France. In 2012, he received his Mechatronics Engineering degree from the National University of Colombia, Bogotá, Colombia. He also earned a master’s degree from the National School of Engineering ENIVL, Blois, France. His research interests include compliance control, redundancy resolution at the torque-level, and physical human-robot interaction. Email address: [email protected]

Pierre Vieyres received a MSC in Electrical engineering from University College London (UK), and a Ph.D. degree in 1990 in biomedical engineering from the University of Tours (France). In 1992, he joined the University of Orléans (France); he is currently a full time Professor in the Robotic Team of the laboratory PRISME. Since 1995, he has been involved in the development of robots for the medical field and especially for tele-echography. He was the Project Manager of the OTELO European project and the national PROSIT project on robotized tele-echography. His activities focus on the development of haptic systems and collaborative robots. Email address: [email protected] Gérard Poisson is Professor at Orléans University (France) and researcher at PRISME Laboratory. He obtained the French Agrégation of Mechanics in 1980 and the PhD in robotics at Orléans University in 1994. He is currently director of the Bourges Institute of Technology (IUT) and deputy director of PRISME Laboratory. His research interests lie on various areas of robotics, including mechatronics integration, mechanical systems design and optimization for robots, perception of the environment, and Human-robot interaction. He was involved in projects on industrial, mobile or medical robotics and author of 100 publications. These last years he has been specifically involved in the study and the design of dedicated devices for robotized tele-echography. Email address: [email protected]