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 sub-tasks, 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 higher-priority tasks, if feasible, and tries to accomplish the lower-priority 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 null-space projection in both dynamic [1, 2] and kinematic  control architectures.
A first classification among tasks can be made with respect to the control objective that they express: equality-based tasks aim to bring the task to a specific desired value, for instance to move the end-effector 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]. Set-based 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]. Task-priority frameworks have been extended to handle also set-based 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 : safety-related, operational and optimization tasks. Safety tasks such as obstacle avoidance or mechanical joint limits  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 low-priority optimization task for each one of the safety-related, high-priority task, aiming to minimize the number of transitions between their activation and deactivation states. Given the null-space projection method, the activation of a high-priority task affects the operational task potentially deviating it from the desired value. In this perspective, minimizing the activation of all the safety-related tasks allows the system to better execute the operational task.
Inspired by the work , where the set-based multi-task priority framework  is used to handle, in simulation only, the kinematic singularity of a snake-like robot setting a proper task at two priority level simultaneously, in this work we extend that idea in order to handle several set-based tasks. In addition, we prove its practical efficiency implementing it experimentally on a 7 DOF Jaco anthropomorphic arm.
Ii Set-Based Task-Priority 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 task-space velocity and the system velocity is :
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 (Closed-Loop Inverse Kinematics) algorithm:
where is a positive-definite 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:
that selects among all the possible solutions the one that minimizes the joint velocity norm. In this way Eq. 2 becomes:
where is the Moore-Penrose pseudoinverse of the Jacobian matrix, defined as:
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:
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 (Singularity-Robust Multi-Task Priority) framework :
In [15, 16] the SRMTP Inverse Kinematics framework has been extended to handle an arbitrary number of tasks, by resorting to the NSB (Null Space-based Behavioral) control. Given a hierarchy composed by tasks sorted by priority level, the solution is computed as:
where is the null space projector of the augmented Jacobian matrix defined as:
The NSB algorithm has been developed to handle equality-based tasks, thus control objectives in which the goal is to bring the task value to a specific one, e.g. moving the arm end-effector 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 set-based tasks or inequality constraints. A set-based task can be seen as an equality-based one which gets active or inactive depending on the operational conditions. In particular, it is necessary to set different reference values for each set-based 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 equality-based task with desired value equal to the corresponding safety threshold:
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 set-based task value, otherwise if the solution would decrease it. In this way, can be deactivated if
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 set-based task value over time and the corresponding , (green dashed lines) and , (red-dashed 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 .
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 set-based 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 higher-priority 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 high-priority safety tasks. In particular, it would be desirable that the control algorithm tries to push a high-priority 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 Set-Based Multi-Task Priority framework an equality-based optimization task should be added in the hierarchy for each one of the set-based high-priority tasks, at a low-priority level with desired value:
greater than the maximum task value if the corresponding set-based task has a lower threshold
lower that the minimum task value if the corresponding set based task has an upper threshold
equal to the mid-point between the minimum and maximum thresholds if the corresponding set-based task has both of them
obtaining the hierarchy shown in Fig. 2. This kind of approach leads to the minimization of the high-priority 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 :
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 high-priority set-based 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 high-priority task) while tries to maximize the manipulability measure during all the trajectory even when the corresponding set-based task in inactive (for the effect of the low-priority task), without interfering with the operational one.
The joint limit task is usually used for avoiding self-collisions, and it can be seen as a high-priority set-based 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 equality-based task in which the desired value is:
In this way the joints are pushed toward the middle of the chosen limits while the end-effector follows the desired trajectory, minimizing the activation of the high-priority set-based 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 high-priority set-based tasks, followed by the corresponding experiment in which we add also the low-priority optimization tasks.
Iv-a First case study: joint limits task
The desired path for the end-effector 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 end-effector position task, obtaining the following hierarchy:
Joint limits (set based)
End-effector 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 end-effector.
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:
High-priority joint limits (set based)
End-effector position (equality)
Low-priority 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 lower-priority 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 end-effector 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 end-effector does not track perfectly the desired path because two joint limits get active anyway.
Iv-B 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 end-effector is shown in Fig. 7.
It is a simple straight line (blue-dashed) 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:
High-priority arm manipulability (set based)
End-effector 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:
High-priority arm manipulability (set based)
End-effector position and orientation (equality)
Low-priority arm manipulability (optimization)
Figure 10 shows the measure of manipulability with the chosen threshold for the primary manipulability task. The desired value for the low-priority 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 end-effector 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 high-priority manipulability task the executed path deviates from the desired one given the activation of the set-based task. In the second case it does not get active, and the end-effector can track the desired path much better.
In this paper we have shown a method for improving the tracking capabilities of the operational tasks in presence of higher-priority 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 safety-tasks, 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 set-based safety tasks.
This work was supported by the European Community through the project AEROARMS(H2020-635491).
-  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, vol. 34, no. 11, pp. 1385–1400, 2015. [Online]. Available: https://doi.org/10.1177/0278364914566516
-  A. Dietrich, C. Ott, and A. Albu-Schaffer, “Multi-objective compliance control of redundant manipulators: Hierarchy, control, and stability,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2013, pp. 3043–3050.
-  B. Siciliano, “Kinematic control of redundant robot manipulators: A tutorial,” Journal of intelligent and robotic systems, vol. 3, no. 3, pp. 201–212, 1990.
-  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.
-  Y. Nakamura, H. Hanafusa, and T. Yoshikawa, “Task-priority based redundancy control of robot manipulators,” The International Journal of Robotics Research, vol. 6, no. 2, pp. 3–15, 1987.
-  S. Chiaverini, “Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators,” IEEE Transactions on Robotics and Automation, vol. 13, no. 3, pp. 398–410, 1997.
-  O. Kanoun, F. Lamiraux, and P. Wieber, “Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task,” IEEE Transactions on Robotics, vol. 27, no. 4, pp. 785–792, 2011.
-  A. Escande, N. Mansard, and P.-B. Wieber, “Hierarchical quadratic programming: Fast online humanoid-robot motion generation,” International Journal of Robotics Research, vol. 33, no. 7, pp. 1006–1028, 2014.
-  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.
-  S. Moe, G. Antonelli, A. Teel, K. Pettersen, and J. Schrimpf, “Set-based tasks within the singularity-robust multiple task-priority inverse kinematics framework: General formulation, stability analysis and experimental results,” Frontiers in Robotics and AI, vol. 3, p. 16, 2016.
-  P. D. Lillo, F. Arrichiello, G. Antonelli, and S. Chiaverini, “Safety-related tasks within the set-based task-priority inverse kinematics framework,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018.
-  A. M. Zanchettin, N. M. Ceriani, P. Rocco, H. Ding, and B. Matthias, “Safety in human-robot collaborative manufacturing environments: Metrics and control,” IEEE Transactions on Automation Science and Engineering, vol. 13, no. 2, pp. 882–893, 2016.
-  J. Sverdrup-Thygeson, S. Moe, K. Y. Pettersen, and J. T. Gravdahl, “Kinematic singularity avoidance for robot manipulators using set-based manipulability tasks,” in IEEE Conference on Control Technology and Applications (CCTA). IEEE, 2017, pp. 142–149.
-  B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: modelling, planning and control. Springer Science & Business Media, 2010.
-  G. Antonelli, F. Arrichiello, and S. Chiaverini, “The NSB control: a behavior-based approach for multi-robot systems,” Paladyn Journal of Behavioral Robotics, vol. 1, no. 1, pp. 48–56, 2010.
-  ——, “The Null-Space-based 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/s11370-007-0002-3
-  F. Arrichiello, P. Di Lillo, D. Di Vito, G. Antonelli, and S. Chiaverini, “Assistive robot operated via P300-based brain computer interface,” in IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp. 6032–6037.
-  T. Yoshikawa, “Manipulability and redundancy control of robotic mechanisms,” in IEEE International Conference on Robotics and Automation, vol. 2. IEEE, 1985, pp. 1004–1009.