I Introduction
Robotic systems are requested to perform more and more complex operations in all kind of environments, leading to flexible control architectures that allow them to adapt to the particular situation in a reactive manner. The most widely used approach is to split the entire operation in several elementary control objectives, implemented as subtasks, possibly to be performed simultaneously. The potential conflict among tasks is resolved by setting a priority and computing the resulting motion commands that assure the achievement of the higherpriority tasks, if feasible, and tries to accomplish the lowerpriority ones as much as possible given the constraints imposed by the more important tasks. This approach has been widely applied exploiting the system redundancy and the nullspace projection in both dynamic [1, 2] and kinematic [3] control architectures.
A first classification among tasks can be made with respect to the control objective that they express: equalitybased tasks aim to bring the task to a specific desired value, for instance to move the endeffector of a manipulator to a certain position and orientation. Most of the the main redundancy resolution algorithms in literature have been developed to handle this kind of tasks [4, 5, 6]. Setbased tasks, or inequality constraints are tasks in which the control objective is to keep the task value in an interval, i.e., above a lower threshold and below an upper threshold. In this category lie tasks such as the obstacle avoidance, the joint limit avoidance and the arm manipulability. Currently one the most popular approaches to handle this kind of tasks is to express the inverse kinematics problem as a sequence of QP (Quadratic Programming) problems [7, 8]. Taskpriority frameworks have been extended to handle also setbased tasks in [9, 10].
The choice of the prioritized order of the tasks within the hierarchy has a major importance and strongly affects the behavior of the system, thus it is useful to divide them in three categories and assign them a decreasing priority level [11]: safetyrelated, operational and optimization tasks. Safety tasks such as obstacle avoidance or mechanical joint limits [12] have to be necessarily set at an higher priority level with respect to the operational tasks, as they assure the integrity of the system and of the environment in which it operates. At the lowest priority level there are the optimization tasks that help in increasing the efficiency of the operation, but they are are not strictly necessary for its accomplishment.
In this paper we propose a method for increasing the performances of a robotic system by setting proper optimization tasks together with the necessary safety and operational ones. The idea is to set a lowpriority optimization task for each one of the safetyrelated, highpriority task, aiming to minimize the number of transitions between their activation and deactivation states. Given the nullspace projection method, the activation of a highpriority task affects the operational task potentially deviating it from the desired value. In this perspective, minimizing the activation of all the safetyrelated tasks allows the system to better execute the operational task.
Inspired by the work [13], where the setbased multitask priority framework [10] is used to handle, in simulation only, the kinematic singularity of a snakelike robot setting a proper task at two priority level simultaneously, in this work we extend that idea in order to handle several setbased tasks. In addition, we prove its practical efficiency implementing it experimentally on a 7 DOF Jaco anthropomorphic arm.
Ii SetBased TaskPriority Inverse Kinematics
For a general robotic system with
DOF (Degrees of Freedom), the state is described by the joint values
. Defining a task as a generic dimensional control objective as a function of the system state , the inverse kinematics problem consists in finding the vector that brings to a desired value . The linear mapping between the taskspace velocity and the system velocity is [14]:(1) 
where is the task Jacobian matrix, and is the system velocity vector. Thus, starting from an initial configuration, in case that meaning that the number of DOF of the system is equal to the task dimension, the joint increment needed to bring the task value closer to the desired one can be computed by resorting to the CLIK (ClosedLoop Inverse Kinematics) algorithm:
(2) 
where is a positivedefinite matrix of gains, is the desired task velocity and is the task error.
A robotic system is defined redundant if , thus if it has more DOF that the required ones for the accomplishment of a certain task. In this case the Jacobian matrix is not invertible anymore and multiple possible solutions for Eq. 1 exist. The system can be solved imposing a constrained minimization problem, in which the cost function is:
(3) 
that selects among all the possible solutions the one that minimizes the joint velocity norm. In this way Eq. 2 becomes:
(4) 
where is the MoorePenrose pseudoinverse of the Jacobian matrix, defined as:
(5) 
In general the solution of Eq. 4 lies is the subspace , and in the redundant case its orthogonal complement can be exploited to add other components that would not affect the accomplishment of the task. For this reason the general solution can be written as:
(6) 
where:
is the null space projector and is an arbitrary vector that can be used to minimize or maximize a scalar value by setting
where is a gain and is a secondary objective function.
Another useful exploitation is to define a second task with a specific desired value and compute the solution that accomplishes both the tasks. Unfortunately this solution may not exist due to the infeasibility of their simultaneous resolution. In this case it is necessary to define a priority between the tasks and compute the solution that achieves the primary one while minimizes the error on the secondary one. Given two tasks and it is possible to compute the system velocities and that accomplish them separately using Eq. 4. The composition of the two tasks solutions and can be performed by resorting to the SRMTP (SingularityRobust MultiTask Priority) framework [6]:
(7) 
In [15, 16] the SRMTP Inverse Kinematics framework has been extended to handle an arbitrary number of tasks, by resorting to the NSB (Null Spacebased Behavioral) control. Given a hierarchy composed by tasks sorted by priority level, the solution is computed as:
(8) 
where is the null space projector of the augmented Jacobian matrix defined as:
(9) 
The NSB algorithm has been developed to handle equalitybased tasks, thus control objectives in which the goal is to bring the task value to a specific one, e.g. moving the arm endeffector to a target position and orientation. However, several control objectives may require their value to lie in an interval, i.e. above a lower threshold and below an upper threshold. These are usually called setbased tasks or inequality constraints. A setbased task can be seen as an equalitybased one which gets active or inactive depending on the operational conditions. In particular, it is necessary to set different reference values for each setbased task: physical thresholds (), safety thresholds (), and activation thresholds (). When the task value reaches an activation threshold, it is added to the task hierarchy as a new equalitybased task with desired value equal to the corresponding safety threshold:
(10) 
Then it can be deactivated when the solution of the hierarchy that contains only the other tasks would push its value toward the valid set. Defining as the Jacobian matrix of , if the solution would increase the setbased task value, otherwise if the solution would decrease it. In this way, can be deactivated if
(11) 
or
(12) 
Active[1pt][1pt][0.6]Active Inactive[1pt][1pt][0.6]Inactive
sigmal[1pt][1pt][0.8] sigmasl[1pt][1pt][0.8] sigmau[1pt][1pt][0.8] sigmasu[1pt][1pt][0.8] task value[1pt][1pt][0.8]task value
Figure 1 shows a generic setbased task value over time and the corresponding , (green dashed lines) and , (reddashed lines). The background color highlights the activation (magenta) and deactivation (yellow) state. At the beginning, the task is inactive, as its value lies within the valid interval, thus the hierarchy contains only the other tasks. As soon as its value reaches , it gets activated and added to the current hierarchy. The corresponding solution brings the task value to with a time constant proportional to the task gain. It remains at that threshold until condition (11) or (12) are satisfied. From that point the task is deactivated and it is removed from the hierarchy. The same happens with the upper thresholds and . For more details about the activation/deactivation algorithm see [17].
Iii Optimization tasks handling
The priority order among the tasks in the hierarchy strongly affects the behavior of the system during the execution of a certain operation. All the setbased tasks related to the safety of the system, such as the mechanical joint limits and the obstacle avoidance, have to be necessarily placed at the top priority level. The execution of the operational task is constrained to the fulfillment of an active higherpriority task and, in case of conflict between them, it leads to a deviation of the operational task from the desired trajectory. Optimization tasks such as the maximization of the arm manipulability can be placed at a lower priority level with respect to the operational one, due to the fact that they are not strictly necessary for the accomplishment of the operation. The idea that we propose in this work is to define proper optimization tasks aiming to minimize the activation/deactivation of highpriority safety tasks. In particular, it would be desirable that the control algorithm tries to push a highpriority task further away from the imposed minimum/maximum limits even when it is not active, exploiting the system redundancy. In order to implement this method in the SetBased MultiTask Priority framework an equalitybased optimization task should be added in the hierarchy for each one of the setbased highpriority tasks, at a lowpriority level with desired value:

greater than the maximum task value if the corresponding setbased task has a lower threshold

lower that the minimum task value if the corresponding set based task has an upper threshold

equal to the midpoint between the minimum and maximum thresholds if the corresponding setbased task has both of them
obtaining the hierarchy shown in Fig. 2. This kind of approach leads to the minimization of the highpriority tasks activation and improves the system performances in tracking the operational task, always assuring that the safety thresholds are respected during the motion. s1[1pt][1pt][1.0]safety task s2[1pt][1pt][1.0]safety task sn[1pt][1pt][1.0]safety task p1[1pt][1pt][1.0]operational task so1[1pt][1pt][1.0]optimization task so2[1pt][1pt][1.0]optimization task son[1pt][1pt][1.0]optimization task
In this work we take into account two kind of tasks: the arm manipulability and the joint limits. The measure of manipulability [18]:
goes to zero when the manipulator reaches a singular configuration, as loses rank. For this reason it can be seen as a distance from a singular configuration.It is possible to add it to a hierarchy as a highpriority setbased task, defining a minimum threshold that the manipulator cannot exceed during the movement. For this work the task Jacobian is computed numerically following the Algorithm 1.
Setting it at a lower priority with respect to the operational task implies that it is accomplished only when it is not in conflict with the primary task, and it can be used as a maximization control objective: choosing the desired value higher than the maximum measure of manipulability that the arm can exhibit the resulting behavior is the same as applying Eq. 6, thus the arm follows the desired trajectory trying to maximize the manipulability measure.
Merging the aforementioned behaviors by including in the hierarchy two manipulability tasks, one at lower priority and one at a higher priority with respect to the operational one, the resulting behavior is that the arm never reaches a singular configuration (for the effect of the highpriority task) while tries to maximize the manipulability measure during all the trajectory even when the corresponding setbased task in inactive (for the effect of the lowpriority task), without interfering with the operational one.
The joint limit task is usually used for avoiding selfcollisions, and it can be seen as a highpriority setbased task with a lower threshold and an upper threshold that constraints its movement in a feasible set of values. The task value is simply the th joint position while the Jacobian is a row vector with a at the th column and zeros at the other ones. The corresponding optimization task is an equalitybased task in which the desired value is:
In this way the joints are pushed toward the middle of the chosen limits while the endeffector follows the desired trajectory, minimizing the activation of the highpriority setbased task.
Iv Experimental results
In this section experimental results on a 7DOF Kinova Jaco manipulator that prove the effectiveness of the proposed approach are shown. In the first case study we focus the attention on the joint limit tasks, while the second one takes into account the manipulability tasks. In both case studies we first perform the experiment with the hierarchy that contains only the highpriority setbased tasks, followed by the corresponding experiment in which we add also the lowpriority optimization tasks.
Iva First case study: joint limits task
The desired path for the endeffector is shown in Fig. 3. It is composed by four waypoints with a square shape keeping constant the coordinate of the arm base frame. Upper and lower limits, intentionally chosen in order to get active during the motion, on six joints have been set and added at a high priority with respect to the endeffector position task, obtaining the following hierarchy:

Joint limits (set based)

Endeffector position (equality)
Figure 4 shows the joint positions over time together with the imposed limits. Notice that the seventh joint position is not reported because it is not included in the hierarchy as it does not give any contribution to the position of the endeffector.
It is clear that the joints 1,3,4,5 and 6 reach one of the limits during the movement, activating the corresponding task that stops their motion.
For the second experiment the implemented hierarchy is:

Highpriority joint limits (set based)

Endeffector position (equality)

Lowpriority joint optimization (optimization)
The starting configuration, the desired sequence of waypoints and the the imposed joint limits are the same of the previous experiment and Fig. 5 shows the joint positions.
It is possible to notice that this time only two joints reach the limits, while joints 3, 5 and 6 benefit from the lowerpriority optimization tasks, being further from the limits with respect to the previous experiment. Figure 6 shows a 3D representation of the executed path for the two experiments. The activation of the joint limit tasks in the first experiment makes the endeffector deviate from the desired path, while in the second one it follows the desired path clearly better given that less higher priority tasks get active during the movement. The endeffector does not track perfectly the desired path because two joint limits get active anyway.
IvB Second case study: manipulability task
The second case study takes into account the behavior of the system when two manipulability tasks are added to the hierarchy with different priority order. The desired path for the endeffector is shown in Fig. 7.
It is a simple straight line (bluedashed) that starts from the yellow circle and ends on the green circle, to be followed forwards and backwards with a constant orientation. The red circle corresponds to the configuration depicted in Fig. 8, in which the measure of manipulability reaches a very low value ().
In the first experiment the task hierarchy is:

Highpriority arm manipulability (set based)

Endeffector position and orientation (equality)
Figure 9 shows the measure of manipulability over time. The task gets active two times, when the desired trajectory reaches the red circle, and the control algorithm effectively avoids the singular configuration keeping the measure of manipulability above the chosen threshold.
Let us now add a second manipulability task at a lower priority while following the same path, resulting in the following hierarchy:

Highpriority arm manipulability (set based)

Endeffector position and orientation (equality)

Lowpriority arm manipulability (optimization)
Figure 10 shows the measure of manipulability with the chosen threshold for the primary manipulability task. The desired value for the lowpriority manipulability task is set at , which is greater than the maximum value that the arm can exhibit.
It is clear that the lower priority manipulability task maximizes the value: the result is that when the desired trajectory reaches for the first time the singular configuration the corresponding task gets active, but it deactivates very quickly with respect to the previous case. Additionally, when the endeffector returns to the initial position the task does not even activate, because the maximization of the manipulability measure during the trajectory has rearranged the configuration of the arm, basically changing the position of the elbow. Figure 11 shows a comparison of the second part of the executed path between the two experiments, from the green circle to the yellow circle.
In the experiment performed with the hierarchy containing only the highpriority manipulability task the executed path deviates from the desired one given the activation of the setbased task. In the second case it does not get active, and the endeffector can track the desired path much better.
V Conclusions
In this paper we have shown a method for improving the tracking capabilities of the operational tasks in presence of higherpriority safety tasks in the hierarchy. We have first described the inverse kinematics framework that allows to define different kind of tasks and to sort them in priority. Then we have discussed how the choice of proper optimization tasks at a lower priority level for each one of the safety tasks can minimize the activation of the safetytasks, leading to a better execution of the operational ones. Experimental results on a 7DOF Kinova Jaco arm proved the effectiveness of the proposed method on two different kind of setbased safety tasks.
Acknowledgments
This work was supported by the European Community through the project AEROARMS(H2020635491).
References
 [1] A. Dietrich, C. Ott, and A. AlbuSchäffer, “An overview of null space projections for redundant, torquecontrolled robots,” The International Journal of Robotics Research, vol. 34, no. 11, pp. 1385–1400, 2015. [Online]. Available: https://doi.org/10.1177/0278364914566516
 [2] A. Dietrich, C. Ott, and A. AlbuSchaffer, “Multiobjective compliance control of redundant manipulators: Hierarchy, control, and stability,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2013, pp. 3043–3050.
 [3] B. Siciliano, “Kinematic control of redundant robot manipulators: A tutorial,” Journal of intelligent and robotic systems, vol. 3, no. 3, pp. 201–212, 1990.
 [4] B. S. J. Slotine, “A general framework for managing multiple tasks in highly redundant robotic systems,” in proceeding of 5th International Conference on Advanced Robotics, vol. 2, 1991, pp. 1211–1216.
 [5] Y. Nakamura, H. Hanafusa, and T. Yoshikawa, “Taskpriority based redundancy control of robot manipulators,” The International Journal of Robotics Research, vol. 6, no. 2, pp. 3–15, 1987.
 [6] S. Chiaverini, “Singularityrobust taskpriority redundancy resolution for realtime kinematic control of robot manipulators,” IEEE Transactions on Robotics and Automation, vol. 13, no. 3, pp. 398–410, 1997.
 [7] O. Kanoun, F. Lamiraux, and P. Wieber, “Kinematic control of redundant manipulators: Generalizing the taskpriority framework to inequality task,” IEEE Transactions on Robotics, vol. 27, no. 4, pp. 785–792, 2011.
 [8] A. Escande, N. Mansard, and P.B. Wieber, “Hierarchical quadratic programming: Fast online humanoidrobot motion generation,” International Journal of Robotics Research, vol. 33, no. 7, pp. 1006–1028, 2014.
 [9] E. Simetti and G. Casalino, “A novel practical technique to integrate inequality control objectives and task transitions in priority based control,” Journal of Intelligent & Robotic Systems, vol. 84, no. 1, pp. 877–902, apr 2016.
 [10] S. Moe, G. Antonelli, A. Teel, K. Pettersen, and J. Schrimpf, “Setbased tasks within the singularityrobust multiple taskpriority inverse kinematics framework: General formulation, stability analysis and experimental results,” Frontiers in Robotics and AI, vol. 3, p. 16, 2016.
 [11] P. D. Lillo, F. Arrichiello, G. Antonelli, and S. Chiaverini, “Safetyrelated tasks within the setbased taskpriority inverse kinematics framework,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018.
 [12] A. M. Zanchettin, N. M. Ceriani, P. Rocco, H. Ding, and B. Matthias, “Safety in humanrobot collaborative manufacturing environments: Metrics and control,” IEEE Transactions on Automation Science and Engineering, vol. 13, no. 2, pp. 882–893, 2016.
 [13] J. SverdrupThygeson, S. Moe, K. Y. Pettersen, and J. T. Gravdahl, “Kinematic singularity avoidance for robot manipulators using setbased manipulability tasks,” in IEEE Conference on Control Technology and Applications (CCTA). IEEE, 2017, pp. 142–149.
 [14] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: modelling, planning and control. Springer Science & Business Media, 2010.
 [15] G. Antonelli, F. Arrichiello, and S. Chiaverini, “The NSB control: a behaviorbased approach for multirobot systems,” Paladyn Journal of Behavioral Robotics, vol. 1, no. 1, pp. 48–56, 2010.
 [16] ——, “The NullSpacebased Behavioral control for autonomous robotic systems,” Journal of Intelligent Service Robotics, vol. 1, no. 1, pp. 27–39, Jan. 2008. [Online]. Available: http://dx.doi.org/10.1007/s1137000700023
 [17] F. Arrichiello, P. Di Lillo, D. Di Vito, G. Antonelli, and S. Chiaverini, “Assistive robot operated via P300based brain computer interface,” in IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp. 6032–6037.
 [18] T. Yoshikawa, “Manipulability and redundancy control of robotic mechanisms,” in IEEE International Conference on Robotics and Automation, vol. 2. IEEE, 1985, pp. 1004–1009.