# Set-Based Tasks within the Singularity-Robust Multiple Task-Priority Inverse Kinematics Framework: General Formulation, Stability Analysis, and Experimental Results

^{1}Center for Autonomous Marine Operations and Systems (AMOS), Department of Engineering Cybernetics, Norwegian University of Science and Technology, Trondheim, Norway^{2}Department of Electrical and Information Engineering, University of Cassino and Southern Lazio, Cassino, Italy^{3}Department of Electrical Engineering, University of California Santa Barbara, Santa Barbara, CA, USA^{4}Department of Engineering Cybernetics, Norwegian University of Science and Technology, Trondheim, Norway

Inverse kinematics algorithms are commonly used in robotic systems to transform tasks to joint references, and several methods exist to ensure the achievement of several tasks simultaneously. The multiple task-priority inverse kinematics framework allows tasks to be considered in a prioritized order by projecting task velocities through the null spaces of higher-priority tasks. This paper extends this framework to handle set-based tasks, i.e., tasks with a range of valid values, in addition to equality tasks, which have a specific desired value. Examples of set-based tasks are joint limit and obstacle avoidance. The proposed method is proven to ensure asymptotic convergence of the equality task errors and the satisfaction of all high-priority set-based tasks. The practical implementation of the proposed algorithm is discussed, and experimental results are presented where a number of both set-based and equality tasks have been implemented on a 6 degree of freedom UR5, which is an industrial robotic arm from Universal Robots. The experiments validate the theoretical results and confirm the effectiveness of the proposed approach.

## 1. Introduction

The desired trajectory of robotic systems is typically given in the task space. In particular, this trajectory often describes the desired position trajectory of the end effector, which is then given in the Cartesian space. For control of robotic systems, the desired trajectory needs to be mapped from this task space to a reference trajectory in the joint space in which the actuators provide their input. In general, this topic falls within the *inverse kinematics* control problem. In most cases, the complexity of the problem requires numerical methods to solve the mapping (Siciliano et al., 2009).

Robotic systems with a large number of degrees of freedom (DOFs) are commonly used for industrial purposes (Siciliano et al., 2009) and are becoming increasingly important within a variety of domains, including humanoid robots (Escande et al., 2014) and unmanned vehicles, such as underwater (Simetti et al., 2013; Antonelli, 2014) and aerial systems (Baizid et al., 2015). Having developed beyond the structured and predictable industrial environment, robotic control algorithms are required to handle real-time trajectory generation. This requirement imposes the use of differential approaches for practical purposes, both at the kinematic and dynamic levels. In Whitney (1969), the use of the pseudoinverse is first recognized as a promising tool to solve the inverse kinematics problem at the kinematic level in robotic applications.

A robotic system is defined as *redundant* when it possesses more DOFs than those strictly required to solve its task, in which case-specific approaches need to be used (Chiaverini et al., 2008). Additional control possibilities then arise in terms of utilizing the “excess” DOFs by adding other tasks to be controlled simultaneously. This is an important possibility in unstructured or non-repetitive environments, as these require the algorithms to handle additional control objectives, such as staying within the mechanical joint limits, avoidance of obstacles, control of the orientation of directional sensors, and maximizing arm manipulability, in addition to the main control objectives. Note that joint limit tasks may be considered as a special case where the task space and the configuration space are identical. Hence, frameworks that map from configuration space to task space may still be utilized for these tasks.

In a task-priority framework, the different control objectives are embedded with a priority relative to their respective importance. The importance of prioritizing between the control objectives can arise from several reasons. Safety, for example, may be considered to be of supreme importance if the environment is shared with human operators. Thus, tracking of the end-effector trajectory may actually be assigned a lower priority in the rank of importance. These considerations lead to solutions as in Liégeois (1977), where the null-space projector is considered in the solution to achieve secondary control objectives afforded by a gradient-based approach. Secondary objectives are defined and handled in a task-priority approach in Maciejewski and Klein (1985) and Nakamura et al. (1987). The work (Siciliano and Slotine, 1991) extends this approach to multiple tasks. In Chiaverini (1997), a different approach is introduced, which guarantees singularity robustness with respect to algorithmic singularities. This approach is further extended to multiple tasks in priority and analyzed in Mansard and Chaumette (2007) and Antonelli (2009), and it is referred to as the singularity-robust multiple task-priority inverse kinematics framework.

Notice that the term *hierarchy* and *priority* are often used as synonyms in the literature. In this paper, the latter will be used.

The singularity-robust multiple task-priority inverse kinematics framework has been developed for *equality tasks*, which are tasks that assign an exact desired value to the controlled variable (e.g., the end-effector configuration). However, for a general robotic system, several goals may be described as *set-based tasks*, i.e., tasks with a desired interval [*area of satisfaction* (Escande et al., 2014)]. Two classic, yet vital examples are joint limits and obstacle avoidance. Additional examples are manipulability, dexterity, and field of view (for directional sensors, such as a camera mounted on the end effector). Set-based tasks are often referred to as *inequality* or *unilateral* constraints in the literature.

As recognized in Kanoun et al. (2011), inclusion of inequality constraints is still missing for the singularity-robust multiple task-priority inverse kinematics framework. This paper aims to fill that gap. In particular, in this paper, we will present a method that allows a general number of scalar set-based tasks to be handled with a given priority within a number of equality tasks. A preliminary description of the idea is given in Antonelli et al. (2015), and Moe et al. (2015a) present some experimental results of the proposed algorithm. It is worth noticing that, with respect to similar approaches presented in the literature, this is the very first one with a formal stability analysis, presented in Moe et al. (2015b). The contribution of this paper is to streamline and present the previous conference contributions in a unified manner and to address how set-based tasks that are given lower priority in the task hierarchy should be handled. Furthermore, new experimental results are presented.

This paper is organized as follows. Section 2 presents relevant literature and alternative approaches to handle set-based tasks. Section 3 gives a brief introduction to the singularity-robust multiple task-priority inverse kinematics framework that is the basis for the presented method, and a closer definition of set-based tasks. The proposed method is described in Section 4, both for high-priority and low-priority set-based tasks and a combination of the two, and it is analyzed with respect to stability in Section 5. Finally, Section 6 considers the practical implementation of the suggested algorithm, and Section 7 presents experimental results performed on a UR5 manipulator. Conclusion and future work are given in Section 8.

## 2. Relevant Literature

In Kanoun et al. (2011) and Simetti et al. (2013), it is pointed out that in order to handle set-based tasks, these are often transformed into equality constraints or un-proper potentials, which leads to over-constraining the original control problem.

One possible way to systematically handle tasks arranged in priority with set-based and equality objectives is to transform the inverse kinematics problem into a quadratic programing (QP) or a similar optimization problem (Ioannou and Sun, 1996), thereby preventing the tasks from being directly included in the multiple task-priority inverse kinematics algorithm unlike the approach suggested in this paper. One of the first solutions using this approach is given by Faverjon and Tournassoud (1987), generalized by Kanoun et al. (2011), and further improved by de Lasa et al. (2010) and Escande et al. (2014) in terms of convergence time. In de Lasa et al. (2010), however, set-based tasks can be considered only as high priority tasks. In Azimian et al. (2014), the inequality constrains are transformed to an equality constraint by the use of proper slack variables. The optimization problem is then modified in order to minimize the defined slack variables together with the task errors in a task-priority architecture.

Set-based tasks may also be embedded in the control problem by assigning virtual forces that push the robot away from the set boundaries. This idea was first proposed in Khatib (1986) and then used in several following approaches. However, when resorting to virtual forces/potentials, satisfaction of the boundaries can not be analytically guaranteed and often the overall control architecture falls within the paradigm of *cooperative behavioral control*, which is possibly affected with drawbacks described in Antonelli et al. (2008). In Simetti et al. (2013), there is a systematic method for handling set-based tasks within the framework of potential fields. In particular, the key aspect is to resort to finite-support smooth potential fields for representing set-based objectives and activation functions. Experimental results with an underwater vehicle-manipulator system are provided. During the activation of the smoothing functions, however, strict priority among the tasks is lost, which is a common drawback when smoothing functions are introduced to avoid discontinuities also in alternative approaches.

In Mansard et al. (2009), set-based tasks are handled within a framework of transitions between solutions. In particular, the transition is necessary to handle the unilateral constraint and avoid discontinuities. Using the damped least-square inverse of the Jacobian, the proposed algorithm does not respect priorities during transitions. In practice, the proposed solution is a way to handle the algorithm given in Siciliano and Slotine (1991) by resorting to homotopy.

In adaptive control, projection methods may be used to ensure that the parameter estimates are bounded by using projection methods (Ioannou and Sun, 1996). The method proposed in this paper may be considered as a type of projection method in the sense that we suggest a discontinuous solution for the system velocities based on projecting the solutions. We suggest a method to directly embed the set-based tasks into the singularity-robust multiple task-priority inverse kinematics framework, thereby making it unnecessary to rewrite the problem into an optimization problem or resort to potential fields. By including the set-based tasks directly into this framework, a stability analysis is feasible, and the paper presents a formal stability analysis with explicit assumptions. Furthermore, the strict priority of the tasks is fulfilled at all times.

## 3. Background

### 3.1. Singularity-Robust Multiple Task-Priority Inverse Kinematics

A general robotic system has *n* DOFs. Its configuration is given by the joint values * q* = [

*q*

_{1},

*q*

_{2}, …,

*q*]

_{n}*. It is then possible to express tasks and task velocities in the operational space through forward kinematics and the task Jacobian matrix. Let us define*

^{T}*(*

**σ***t*) ∈ ℝ

*as the task variable to be controlled:*

^{m}with the corresponding differential relationship:

where * J*(

*(*

**q***t*)) ∈ ℝ

^{m×n}is the configuration-dependent analytical task Jacobian matrix and $\dot{\mathit{q}}\left(t\right)\in {\mathbb{R}}^{n}$ is the system velocity. For compactness, the argument

*of tasks and Jacobians are omitted from the equations in this paper.*

**q**Let us first consider a single *m*-dimensional task to be followed, with a defined desired trajectory **σ**_{des}(*t*) ∈ ℝ* ^{m}*. The corresponding joint references

**q**_{des}(

*t*) ∈ ℝ

*for the robotic system may be computed by integrating the locally inverse mapping of equation (2) achieved by imposing minimum-norm velocity (Siciliano, 1990). The following least-squares solution is given:*

^{n}where **J**^{†}, implicitly defined in the above equation for full row rank matrices, is the right pseudoinverse of * J*. In the general case, the pseudoinverse is the matrix that satisfies the four Moore–Penrose conditions [equations (4)–(7)] (Golub and Van Loan, 1996), and it is defined for systems that are not square (

*m*≠

*n*) nor have full rank (Buss, 2009):

Here, * J** denotes the complex conjugate of

*.*

**J**The vector **q**_{des} achieved by taking the time integral of equation (3) is prone to drifting. To handle this, a closed loop inverse kinematics (CLIK) version of the algorithm is usually implemented (Chiaverini, 1997), namely,

where $\tilde{\mathit{\sigma}}\in {\mathbb{R}}^{m}$ is the task error defined as

and $\mathbf{\Lambda}\in {\mathbb{R}}^{m\times m}$ is a positive-definite matrix of gains. This feedback approach reduces the error dynamics to

if $\dot{\mathit{q}}={\dot{\mathit{q}}}_{\text{des}}$ and * J* has full rank, implying that

**JJ**^{†}=

*. Equation (10) describes a linear system with a globally exponentially stable equilibrium point at the equilibrium $\tilde{\mathit{\sigma}}=\mathit{0}$. It is worth noticing that the assumption $\dot{\mathit{q}}={\dot{\mathit{q}}}_{\text{des}}$ is common to all inverse kinematics algorithms (Antonelli, 2008). For practical applications, it requires that the low-level dynamic control loop is faster than the kinematic one.*

**I**In case of system redundancy, i.e., if *n* > *m*, the classic general solution contains a null projector operator (Liégeois, 1977):

where **I*** _{n}* is the (

*n*×

*n*) identity matrix and the vector ${\dot{\mathit{q}}}_{\text{null}}\in {\mathbb{R}}^{n}$ is an arbitrary system velocity vector. It can be recognized that the operator (

**I***−*

_{n}

**J**^{†}

*) projects ${\dot{\mathit{q}}}_{\text{null}}$ in the null space of the Jacobian matrix. This corresponds to generating a motion of the robotic system that does not affect that of the given task.*

**J**For highly redundant systems, multiple tasks can be arranged in priority. Let us consider three tasks that will be denoted with the subscripts 1, 2 and 3, respectively:

For each of the tasks, a corresponding Jacobian matrix can be defined, denoted **J**_{1} ∈ ℝ^{m1×n}, **J**_{2} ∈ ℝ^{m2×n}, and **J**_{3} ∈ ℝ^{m3×n}, respectively. Let us further define the corresponding null-space projectors for the first two tasks as

The *augmented Jacobian* of tasks 1 and 2 is given by stacking the two independent task Jacobians:

The common null-space for tasks 1 and 2 is then defined as

where ${{\mathit{J}}_{12}^{\text{A}}}^{\u2020}$ is the pseudoinverse of ${\mathit{J}}_{12}^{\text{A}}$ that satisfies the four Moore–Penrose conditions [equations (4)–(7)]. By expanding the expression for ${\mathit{J}}_{12}^{\text{A}}$, we see that

so in general,

The following equation then defines the desired joint velocities:

where the definition of ${\dot{\mathit{\sigma}}}_{x,\text{ref}}$ can be easily extrapolated from equation (8) for each task with the corresponding positive-definite matrix ${\mathbf{\Lambda}}_{x}\in {\mathbb{R}}^{{m}_{x}\times {m}_{x}}$. The priority of the tasks follows the numerical order, with **σ**_{1} being the highest-priority task. Equation (21) also implicitly defines the joint velocities ${\dot{\mathit{q}}}_{x,\text{des}}\in {\mathbb{R}}^{n}$ that represent the desired joint velocity corresponding to task **σ*** _{x}* if this was the sole task.

The generalization to *k* tasks is straightforward: equation (21) can be expanded as:

where ${\mathit{N}}_{12\dots \left(k-1\right)}^{\text{A}}$ is the null space of the augmented Jacobian matrix

### 3.2. Set-Based Definitions

The previous section introduced the concept of multiple task-priority inverse kinematic control for a robotic system, as a method to generate reference trajectories for the system configuration that, if satisfied, will result in the successful achievement of several tasks. However, this framework has been developed for equality tasks that have a specific desired value **σ**_{des}(*t*), e.g., the desired end effector position. This paper proposes a method to extend the existing framework to handle set-based tasks, such as the avoidance of joint limits and obstacles, and field of view.

A set-based task is still expressed through forward kinematics equation (1), but the objective is to keep the task in a defined set *D* rather than controlling it to a desired value. Mathematically, this can be expressed as * σ*(

*t*) ∈

*D*∀

*t*rather than

*(*

**σ***t*) =

**σ**_{des}(

*t*). Thus, set-based tasks cannot be directly inserted into the singularity-robust multiple task-priority inverse kinematics framework [equation (22)]. In this paper, we will present a method that allows a general number of scalar set-based tasks to be handled with a given priority within a number of equality tasks.

From here on, equality tasks are denoted with number subscripts and set-based tasks with letters. Furthermore, while equality tasks in general can be multidimensional and are thus described as vectors, set-based tasks are scalar and are therefore not expressed in bold-face, e.g., **σ**_{1} and *σ*_{a}. Finally, only regulation equality tasks are considered, that is equality tasks to guide the system to a stationary value (${\dot{\mathit{\sigma}}}_{\text{des}}\equiv 0$). Finally, it is assumed that the desired joint velocities ${\dot{\mathit{q}}}_{\text{des}}$ are tracked perfectly by the system, so that $\dot{\mathit{q}}={\dot{\mathit{q}}}_{\text{des}}$.

Consider Figure 1. A set-based task *σ* is defined as *satisfied* when it is contained in its valid set, i.e., *σ* ∈ *D* = [*σ*_{min}, *σ*_{max}]. On the boundary of *D*, the task is still satisfied, but it might be necessary to actively handle the task to prevent it from becoming violated. This is elaborated on in Section 4.

**Figure 1. Illustration of valid set D**. The set-based task

*σ*is satisfied in

*D*and violated outside of

*D*.

## 4. Set-Based Singularity-Robust Task-Priority Inverse Kinematics

This section presents the proposed method for incorporating set-based tasks in the singularity-robust multiple task-priority inverse kinematics framework. *High-priority set-based tasks* are defined as all set-based tasks with a higher priority than the highest-priority equality task, whereas *low-priority set-based tasks* have priority after at least one equality task. Section 4.1 describes how high-priority set-based tasks are handled. For simplicity, a system with a single set-based task is considered first, before the general case of *j* set-based tasks is presented. Low-priority set-based tasks offer some additional challenges and are further described in Section 4.2, also for 1 and *j* set-based tasks, respectively. Finally, in Section 4.3, the framework is presented for the completely general case of a combination of high- and low-priority set-based tasks. Note that concrete examples are given in Section 7.

When a set-based task is in the interior of *D* (Figure 1), it should not affect the behavior of the system. All the system DOFs can then be used to fulfill the equality tasks without being limited in any way by the set-based task, which is *inactive*. The proposed algorithm therefore considers only the system’s equality tasks according to equation (33) as long as the resulting solution stays within this desired set or, should it be outside, does not further increase the distance to the set. If this is not the case, the set-based task is actively inserted into the task priority to be handled. A key aspect of the proposed solution is the *tangent cone* and *extended tangent cone* to a set. The tangent cone to the set *D* in Figure 1 at the point *σ* ∈ *D* is defined as

The left-end point in Figure 1 corresponds to *σ*_{min}, in which case the tangent cone is defined as [0,∞). Similarly, the right-end point of Figure 1 represents *σ*_{max}, and in this point, the tangent cone is defined as (−∞, 0]. Note that if $\dot{\mathit{\sigma}}\left(t\right)\in {T}_{D}\left(\mathrm{\sigma}\left(t\right)\right)\forall t\ge {t}_{0}$, then this implies that *σ*(*t*) ∈ *D*∀*t* ≥ *t*_{0}. If *σ* is in the interior of *D*, the derivative is always in the tangent cone, as this is defined as ℝ. If *σ* = *σ*_{min}, the task is at the lower border of the set. In this case, if $\dot{\mathit{\sigma}}\in \left[0,\infty \right)$, then *σ* will either stay on the border, or move into the interior of the set. Similarly, if *σ* = *σ*_{max} and $\dot{\mathrm{\sigma}}\in \left(-\infty ,0\right]$, *σ* will not leave *D*.

We define the extended tangent cone to *D* at the point *σ* ∈ ℝ as

The definition of the extended tangent cone is very similar to the tangent cone, but it is defined for *σ* ∈ ℝ, not just *σ* ∈ *D*. For $\mathrm{\sigma}\notin D$, $\dot{\mathrm{\sigma}}\in {T}_{\mathbb{R},D}$ implies that *σ* either stays constant or moves closer to *D*.

Due to the fact that a set-based task can be either active or inactive, a system with *j* set-based tasks has 2* ^{j}* possible combinations of set-based tasks being active/inactive. These combinations are referred to as “modes” of the system, and the proposed algorithm must switch between modes to fulfill the equality tasks while ensuring that the set-based tasks are not violated. The modes are sorted by increasing restrictiveness. The more set-based tasks that are active in a mode, the more restrictive it is. Hence, in the first mode, no set-based tasks are active. This corresponds to considering only the system equality tasks [equation (22)]. In the 2

*th mode, all set-based tasks are active.*

^{j}Throughout this section, we consider a robotic system with *n* DOFs and *k* equality tasks of *m _{i}* DOFs each for

*i*∈ {1, …,

*k*}. Equality task

*i*is denoted

*, the task error is defined as $\tilde{\mathit{\sigma}}\triangleq {\mathit{\sigma}}_{i,\mathrm{des}}-{\mathit{\sigma}}_{i}$. Furthermore, the system has*

**σ**_{i}*j*set-based tasks. The first and

*j*th set-based tasks are denoted

*σ*

_{a}and

*σ*

_{x}, respectively (where x represents the

*j*th letter of the alphabet). We consider the state-vector

*∈ ℝ*

**z***, where*

^{l}and

Here, **σ**_{sb} and ${\tilde{\mathit{\sigma}}}_{\text{eb}}$ are defined as vectors containing the set-based tasks and the corresponding valid set for * z* in which all set-based tasks are satisfied is defined as

where

**Assumption 1:** When an additional task is considered, the task Jacobian is independent with respect to the Jacobian obtained by stacking all the higher-priority tasks, i.e.,

for *i* ∈ {2, …, ( *j* + *k*)} where, *ρ*(⋅) is the rank of the matrix and *k* is the total number of tasks. Furthermore, the task gains are chosen according to Antonelli (2009). For the specific case of (*j* + *k*) = 3, the task gains are chosen as ${\mathbf{\Lambda}}_{1}={\mathrm{\lambda}}_{1}{\mathit{I}}_{{m}_{1}}$, ${\mathbf{\Lambda}}_{2}={\mathrm{\lambda}}_{2}{\mathit{I}}_{{m}_{2}}$, and ${\mathbf{\Lambda}}_{3}={\mathrm{\lambda}}_{3}{\mathit{I}}_{{m}_{3}}$ for the first, second, and third priority task, respectively, with

where ${\overline{\lambda}}_{\mathit{ij}}$ and ${\underset{\xaf}{\lambda}}_{\mathit{ij}}$ denote the largest and smallest singular value of the matrix **P*** _{ij}*, respectively, and

For practical purposes, Assumption 1 requires that the tasks are compatible. For instance, if the system is given one end-effector position tracking task and one collision avoidance task, and the desired trajectory moves through the obstacle, the tasks are clearly not compatible. In this case, Assumption 1 is not satisfied, and the system will fulfill the highest-priority task.

### 4.1. High-Priority Set-Based Tasks

#### 4.1.1. One Set-Based Task, *k* Equality Tasks

For simplicity, we first consider a robotic system with a single high-priority set-based task *σ*_{a} ∈ ℝ. In this section, we choose *σ*_{a} as a collision avoidance task as an example, with the goal of avoiding a circular obstacle at a constant position **p**_{o} with radius *r* > 0. The task is defined as the distance between the end effector and the obstacle center. The kinematics and Jacobian are given in Antonelli (2014):

Here, **p**_{e} denotes the position of the end effector and * J* is the corresponding position Jacobian. For this specific example, we define

for an ε > *r* and the set *C* as in equation (28) with *j* = 1 and *C*_{1} as defined above. In *C*, the set-based task is limited to [ε, ∞). Thus, the set-based task is always satisfied for * z* ∈

*C*.

For a system with one high-priority set-based task, two modes must be considered:

(1) Ignoring the set-based task and considering only the equality tasks.

(2) Freezing the set-based task as first priority and considering the equality tasks as second priority.

Mode 1 is the “default” solution, whereas mode 2 should be activated only when it is necessary to prevent the set-based task from being violated. Using the multiple task-priority inverse kinematics framework presented in Section 3.1, mode 1 corresponds to equation (22) and results in the following system:

The matrix **M**_{1} is positive definite by Assumption 1 (Antonelli, 2009).

In mode 1, the set-based task evolves freely according to ${\dot{\mathrm{\sigma}}}_{\text{a}}={f}_{12}\left(\mathit{z}\right)$, and the time evolution of * z* should follow the vector field

**f**_{1}(

*) as long as the solution*

**z***stays in*

**z***C*. If following

**f**_{1}(

*) would result in*

**z***leaving the set*

**z***C*, avoiding the obstacle is considered the first priority task and mode 2 is activated. This can only occur on the border of C, that is for

*σ*

_{a}= ε, and when ${\dot{\mathrm{\sigma}}}_{\text{a}}={f}_{12}\left(\mathit{z}\right)<0$. To avoid the obstacle, an equality task with the goal of keeping the current distance to the obstacle is added as the highest-priority task. In this case, the desired task value

*σ*

_{a,des}is equal to the current task value

*σ*

_{a}= ε. Thus, the task error is ${\tilde{\mathit{\sigma}}}_{\text{a}}={\mathrm{\sigma}}_{\text{a},\text{des}}-{\mathrm{\sigma}}_{\text{a}}\equiv 0$. The system is then defined by the following equations:

The matrix **M**_{2} can be seen as a principal submatrix of the general matrix * M* in equation (60) in Antonelli (2009), which is shown to be positive definite given Assumption 1. Thus,

**M**_{2}is also positive definite. Furthermore, as can be seen by equation (44), the joint velocities [equation (43)] ensure that the distance to the obstacle is kept constant (${\dot{\mathrm{\sigma}}}_{\text{a}}=0$).

Let **T*** _{C}*(

*) denote the tangent cone to*

**z***C*at the point

*∈*

**z***C*:

Consider the continuous functions **f**_{1}, **f**_{2} : *C* → ℝ* ^{l}* as defined in equations (42) and (46). The time evolution of

*follows the vector field*

**z**

**f**_{1}(

*) as long as the solution*

**z***stays in*

**z***C*(mode 1). Using Lemma 5.26 (Goebel et al., 2012) on the system $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{1}\left(\mathit{\text{z}}\right)$, we know that such a solution exists when

**f**_{1}(

*) ∩*

**x**

**T***(*

_{C}*) ≠ ∅ for all*

**x***near*

**x***(restricting*

**z***to*

**x***C*). Hence, we define the set

The discontinuous function * f* :

*C*→ ℝ

^{l}then describes our system. The differential equation $\dot{\mathit{\text{z}}}=\mathit{\text{f}}\left(\mathit{\text{z}}\right)$ then corresponds to following **f**_{1} (mode 1) as long as * z* stays in

*C*and following

**f**_{2}(mode 2) otherwise. In mode 2,

*σ*

_{a}is frozen, so the (

*n*+ 1)th element in

**f**_{2}(

*) ≡ 0. Consequently,*

**z**

**f**_{2}(

*) ∈*

**z**

**T***(*

_{C}*) ∀*

**z***∈*

**z***C*. This implies that

*C*is strongly forward invariant for $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{2}\left(\mathit{\text{z}}\right)$, i.e., that

*(*

**z***t*

_{0}) ∈

*C*⇒

*(*

**z***t*) ∈

*C*∀

*t*≥

*t*

_{0}.

In other words, the set *S* contains the points * z* in

*C*such that

**f**_{1}(

*) ∈*

**x**

**T***(*

_{C}*) for*

**x***in*

**x***C*that are near

*. At the border of*

**z***C*,

*σ*

_{a}= ε and the distance between the end effector and the obstacle center is at the minimum allowed value. Therefore, the (

*n*+ 1)th element of

**f**_{1}(

*), corresponding to ${\dot{\mathrm{\sigma}}}_{\text{a}}$, must be zero or positive for*

**z***to stay in*

**z***S*. If it is not, mode 2 is activated, which freezes the distance to the obstacle at the border of

*C*. This remains the solution until following

**f**_{1}(

*) will result in ${\dot{\mathrm{\sigma}}}_{\text{a}}\ge 0$, i.e., that the distance between the end effector and obstacle will remain constant or increase.*

**z**#### 4.1.2. j Set-Based Tasks, k Equality Tasks

We now consider the general case of j high-priority set-based tasks and consider the state vector * z* ∈ ℝ

*and the corresponding valid set as defined in equations (27) and (28), respectively. Note that in the case that a set-based task σ is a joint limit for joint*

^{l}*i*,

*i*∈ {1, …,

*n*}, then

*σ*=

*q*and should therefore not be included in the vector

_{i}

**σ**_{sb}, as it is already included in the state vector through

*. With j set-based tasks, it is necessary to consider the 2*

**q***solutions resulting from all combinations of activating and deactivating every set-based task. These are presented in Table 1.*

^{j}All modes have certain commonalities:

(1) All active set-based tasks are frozen.

(2) Inactive set-based tasks do not affect the behavior of the system.

(3) All matrices **M**_{i} for *i* ∈ {1, …, 2* ^{j}*} are either equal to or a principal submatrix of the general matrix

*in equation (60) in Antonelli (2009). Consequently, given Assumption 1, the matrices*

**M**

**M**_{i}are positive definite.

Consider the corresponding tangent cone to the set *C* in equation (28).

where ${T}_{{C}_{i}}\left(\mathit{\text{z}}\right)$ for *i* ∈ {1, …, *j*} is defined as in equation (24). Defining the sets

the discontinuous equation $\dot{\mathit{\text{z}}}=\mathit{\text{f}}\left(\mathit{\text{z}}\right)$ with * f* :

*C*→ ℝ

*defined as*

^{l}then defines our system.

### 4.2. Low-Priority Set-Based Tasks

#### 4.2.1. One Set-Based Task, k Equality Tasks

For simplicity, we first consider a robotic system with a single low-priority set-based task σ_{a} ∈ ℝ with priority between equality tasks **σ**_{1} and **σ**_{2} and a valid set *σ*_{a} ∈ [*σ*_{a,min}, *σ*_{a,max}].

Consider the state-vector * z* ∈ ℝ

*and the closed set*

^{l}*C*, where

*l*,

*, and*

**z***C*are defined in equations (26)–(28), respectively.

Similar to Section 4.1.1, one set-based task leads to two modes, and in the first mode only the equality tasks are considered. Hence, mode 1 corresponds exactly to equations (39)–(42) and reduces the equality task error dynamics to ${\dot{\tilde{\mathrm{\sigma}}}}_{\text{eb}}=-{\mathit{\text{M}}}_{1}{\tilde{\mathrm{\sigma}}}_{\text{eb}}$. In mode 2, the set-based task is actively handled, and as for the higher-priority set-based tasks in Section 4.1.1, the goal is to freeze it at its current value. Hence, ${\tilde{\mathrm{\sigma}}}_{\text{a}}\equiv 0$ and the joint velocities for mode 2 are given as

In Section 4.1.1, it was shown that mode 2 implies ${\dot{\mathrm{\sigma}}}_{\text{a}}\text{\hspace{0.17em}}=\text{\hspace{0.17em}}0$, so if this mode is activated on the border of *C* (*σ*_{a} = *σ*_{a,min} or *σ*_{a} = *σ*_{a,max}), *σ*_{a} will indeed be frozen and thereby remain in *C*. However, when *σ*_{a} is a low-priority task, the same guarantee cannot be made. Consider the evolution of *σ*_{a} in case of equation (56):

Equation (57) is not exactly equal to zero. Rather, the evolution of *σ*_{a} is influenced by the higher-priority equality task. Thus, lower-priority set-based tasks cannot be guaranteed to be satisfied, as they can not be guaranteed to be frozen at any given time. Even so, they should still be actively handled by attempting to freeze them, as this might result in these tasks deviating less from their valid set than if they are ignored completely. Thus, we define the system mode 2 as equation (56):

Similar to Section 4.1.1, the matrix **M**_{2} is positive definite given Assumption 1.

Consider the continuous functions **f**_{1}, **f**_{2}: *C* → ℝ^{l} as defined in equations (42) and (62). The time evolution of * z* should follow the vector field

**f**_{1}(

*) as long as the solution*

**z***stays in*

**z***C*(mode 1). However, since it cannot be guaranteed that

*stays in*

**z***C*, mode 1 should also be active if

*∉*

**z***C*and following

**f**_{1}(

*) will result in*

**z***keeping its current distance to or moving closer to*

**z***C*. This is mathematically expressed for the system $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{1}\left(\mathit{\text{z}}\right)$ as ${\mathit{\text{f}}}_{1}\left(\mathit{\text{x}}\right)\cap {\mathit{\text{T}}}_{\mathbb{R},C}\left(\mathit{\text{x}}\right)\ne \varnothing $ for all

*near*

**x***, where*

**z**and ${T}_{\mathbb{R},{C}_{1}}\left(\mathit{\text{z}}\right)$ is the extended tangent cone to *C*_{1} in equation (29) at the point σ_{a} as defined in equation (25). Hence, we define

The discontinuous function * f* :

*C*→ ℝ

^{l}

then describes our system. The differential equation $\dot{\mathit{\text{z}}}$ = * f* (

*) then corresponds to following*

**z**

**f**_{1}(

*) (mode 1) if one of three conditions are satisfied:*

**z**(1) σ_{a} ∈ (σ_{a,min}, σ_{a,max})

(2) σ_{a} ≥ σ_{a,max} and $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{1}\left(\mathit{\text{z}}\right)\Rightarrow {\dot{\mathrm{\sigma}}}_{\text{a}}\le 0$

(3) σ_{a} ≤ σ_{a,min} and $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{1}\left(\mathit{\text{z}}\right)\Rightarrow {\dot{\mathrm{\sigma}}}_{\text{a}}\ge 0$.

If none of these conditions hold, mode 2 is activated and $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{2}\left(\mathit{\text{z}}\right)$. In mode 2, the set-based task is actively handled by attempting to freeze it, but since the (*n* + 1)th element in **f**_{2}(* z*) is not identically equal to 0, this cannot be guaranteed. However, as shown in equation (59), as soon as the higher-priority equality task

**σ**_{1}converges to its desired value,

*σ*

_{a}is indeed frozen in mode 2 and is not affected by the lower-priority equality tasks.

#### 4.2.2. j Set-Based Task, k Equality Tasks

In this section, we consider a system with j low-priority set-based tasks. Consider the state-vector * z* ∈ ℝ

*, where l and*

^{l}*are defined in equations (95) and (27), respectively. We assume that the set-based tasks are labeled so that*

**z***σ*

_{b}always has lower priority than

*σ*

_{a}, etc. Furthermore, assume that

*σ*

_{a}has priority after equality task ${\mathrm{\sigma}}_{{p}_{\text{a}}}$ for some

*p*

_{a}= {1, …,

*k*},

*σ*

_{b}has priority after equality task ${\mathrm{\sigma}}_{{p}_{\text{b}}}$ for some

*p*

_{b}= {

*p*

_{a}, …,

*k*}, and so forth. The resulting 2

*modes of the system are presented in Table 2.*

^{j}For example, for a system with *j* = 3 set-based tasks and *k* = 6 equality tasks, *p*_{a} = 3 and *p*_{b} = *p*_{c} = 5, the 2* ^{j}* = 8th mode would be

All matrices **M**_{i} in Table 2 for *i* ∈ {1, … 2* ^{j}*} are either equal to or a principal submatrix of the general matrix

*in equation (60) in Antonelli (2009). Consequently, given Assumption 1, the matrices*

**M**

**M**_{i}are positive definite.

Consider the sets *C*_{1}-*C _{j}* defined by equations (29) and (30) with corresponding extended tangent cones as defined in equation (25). If the

*i*th set-based task is inactive in a given mode

*, the corresponding (*

**f***n*+

*i*)th element of vector field

*(*

**f***) must be in ${T}_{\mathbb{R},{C}_{i}}\left(\mathit{\text{x}}\right)$ for all*

**x***near*

**x***. If the same task is active, there is no requirement to the same element of the vector field. In mode 1, all the set-based tasks are inactive, thus all the (*

**z***n*+ 1) to (

*n*+

*j*) elements of

**f**_{1}(

*) must be in their respective extended tangent cones for this mode to be chosen. In mode 2, only σ*

**z**_{a}is active, so in this case all the (

*n*+ 2) to (

*n*+

*j*) elements of

**f**_{2}(

*) must be in their respective extended tangent cones if mode 2 should be the active mode. Thus, we define*

**z**The discontinuous equation $\dot{\mathit{\text{z}}}\text{\hspace{0.17em}}=\text{\hspace{0.17em}}\mathit{\text{f}}\left(\mathit{\text{z}}\right)$ with * f* : C → ℝ

_{l}defined as

then defines our system.

### 4.3. Combination of High- and Low-Priority Set-Based Tasks

Consider an *n* DOF robotic system with *k* equality tasks of *m _{i}* DOFs for

*i*∈ {1, …,

*k*} and

*j*set-based tasks ∈ ℝ, where

*j*≤

_{x}*j*of them are high priority and

*j*−

*j*are low-priority at any given priority level. Denote that the

_{x}*j*th, (

_{x}*j*+ 1)th, and

_{x}*j*th set-based task as

*σ*

_{x},

*σ*

_{y}, and

*σ*

_{z}, respectively, where

*σ*

_{x}is the last high-priority set-based task,

*σ*

_{y}is the first low-priority set-based task, and

*σ*

_{z}is the lowest priority set-based task [x, y, and z represent the

*j*th, (

_{x}*j*+ 1)th, and

_{x}*j*th letter of the alphabet, respectively]. Consider the state-vector

*∈ ℝ*

**z***, where*

^{l}and

It can be shown that the 2* ^{j}* resulting modes from all possible combinations of active and inactive set-based tasks will reduce the error dynamics of the equality tasks to the form

for *i* = {1, …, 2* ^{j}*}. The analysis is similar to the above sections and all matrices

**M**_{i}are either equal to or a principal submatrix of the general matrix

*in equation (60) in Antonelli (2009). Therefore, by Assumption 1, the matrices*

**M**

**M**_{i}are positive definite. Furthermore, as described in detail in Section 4.1, all the active high-priority set-based tasks are frozen in all modes.

Consider the sets

and

Note that * z* ∈

*C*only implies that all the high-priority set-based tasks are within their respective desired sets. Furthermore, the tangent cones are considered for the high-priority set-based tasks, and the extended tangent cones for the low-priority ones.

The discontinuous equation $\dot{\mathit{\text{z}}}\text{\hspace{0.17em}}=\text{\hspace{0.17em}}\mathit{\text{f}}\left(\mathit{\text{z}}\right)$ with * f* :

*C*→ ℝ

^{l}defined as

then defines our system. Similar to the analysis of the previous sections, in the sets *S _{i}*,

*i*∈ {1, …, 2

*}, the elements of the vector flows*

^{j}

**f**_{i}(

*) corresponding to the high-priority set-based tasks are required to be in their respective tangent cone in all modes, and the elements corresponding to the low-priority set-based tasks are required to be in the corresponding extended tangent cone only in the modes where that task is inactive.*

**z**## 5. Stability Analysis

To study the behavior of the discontinuous differential equations [equation (92)], we look at the corresponding constrained differential inclusion:

where

𝔹 is a unit ball in ℝ^{dim(z)} centered at the origin. $\text{co}\overline{P}$ denotes the closed convex hull of the set *P*, or in other words, the smallest closed convex set containing *P*. The state evolution of * z* is the Krasovskii solution of the discontinuous differential equation (55) [Definition 4.2 (Goebel et al., 2012)].

### 5.1. Convergence of Equality Tasks

The first part of the stability proof considers the convergence of the system equality tasks when switching between 2* ^{j}* possible modes of the system.

If *V* is a continuously differentiable Lyapunov function for $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{i}\left(\mathit{\text{z}}\right)$ for all *i* ∈ {1, …, 2* ^{j}*}, then

*V*is a Lyapunov function for $\dot{\mathit{\text{z}}}\in \mathit{\text{F}}\left(\mathit{\text{z}}\right)$. The following equation holds as all

**f**_{i}(

*) are continuous functions:*

**z**for some $\sum _{i=1}^{{2}^{j}}}\text{\hspace{0.17em}}{\mathrm{\lambda}}_{i}=1,{\mathrm{\lambda}}_{i}\ge 0.$ Consider the Lyapunov function candidate for the equality task errors:

Using equation (95) and the system equations given in Tables 1 and 2 for high-priority and low-priority set-based tasks, respectively, we find that the time derivative of *V* is given by

The convex combination * Q* of positive-definite matrices is also positive definite. Therefore, $\dot{V}$ is negative definite and ${\tilde{\mathrm{\sigma}}}_{{\text{\hspace{0.17em}}}_{\text{eb}}}=\mathbf{\text{0}}$ is a globally asymptotically stable equilibrium point in all modes. Thus, the equality task errors ${\tilde{\mathrm{\sigma}}}_{{\text{\hspace{0.17em}}}_{\text{eb}}}$ asymptotically converge to zero when switching between modes. Furthermore, if

*belongs to a compact set, the equilibrium point ${\tilde{\mathrm{\sigma}}}_{\text{eb}}=\mathbf{\text{0}}$ is exponentially stable.*

**q**### 5.2. Satisfaction of Set-Based Tasks and Existence of Solution

The second part of the stability proof considers the satisfaction of the set-based tasks and the existence of a valid solution.

The analysis made in Section 4.2.1 concluded that the evolution of active low-priority set-based tasks is influenced by the higher-priority equality tasks. Unlike the high-priority set-based tasks they cannot be frozen at any given time, and therefore they cannot be guaranteed to be satisfied. We have defined a closed set *C* in equation (79) within which all high-priority set-based tasks are satisfied at all times. As long as the system solution * z* ∈

*C*, these tasks are not violated.

For a system described by equation (92), $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{1}\left(\mathit{\text{z}}\right)$ as long as the solution * z* ∈

*C*and the lower-priority set-based tasks outside of their respective desired sets do not move further away from them. If the system reaches the boundary of

*C*and remaining in mode 1 would cause

*to leave*

**z***C*, another mode is activated. If neither of the vector fields ${\mathit{\text{f}}}_{1}-{\mathit{\text{f}}}_{{2}^{j}-1}$ will result in

*staying in*

**z***C*, the chosen solution is $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{{2}^{j}}\left(\mathit{\text{z}}\right)$, for which it has been shown that ${\dot{\mathrm{\sigma}}}_{\text{a}}$ to ${\dot{\mathrm{\sigma}}}_{\text{x}}\equiv 0$. Therefore, ${\mathit{\text{f}}}_{{2}^{j}}\left(\mathit{\text{z}}\right)\in {\mathit{\text{T}}}_{C}\left(\mathit{\text{z}}\right)\forall \mathit{\text{z}}\in C$, and

*C*is strongly forward invariant for $\dot{\mathit{\text{z}}}={\mathit{\text{f}}}_{{2}^{j}}\left(\mathit{\text{z}}\right)$. Thus, there will always exist a solution

*∈*

**z***C*and the high-priority set-based tasks are consequently always satisfied.

## 6. Implementation

This section presents the practical implementation of the proposed algorithm and discusses the computational load of running it.

In the stability analysis, only regulation equality tasks are considered, that is tasks with ${\dot{\mathrm{\sigma}}}_{\text{des}}\equiv \mathbf{\text{0}}$. For practical purposes, one can also apply this algorithm for time-varying equality tasks as in equation (21). An example of this is presented in Section 7. Furthermore, for the stability analysis, the lower-priority set-based tasks are handled the same way as the high-priority ones and are attempted frozen. However, as this can not be guaranteed, they might exceed their desired set. With this in mind, for practical purposes, these tasks can be handled by defining *σ*_{des} = *σ*_{max} and *σ*_{des} = *σ*_{min} if *σ* ≥ *σ*_{max} and ≤*σ*_{min}, respectively. Should σ leave the set, the solution will actively attempt to bring *σ* back to the border of its valid set (rather than simply freezing it outside the set) by having a non-zero error feedback *σ* as it does in, for instance, equation (56).

In the implementation, the Boolean function `in_T_RC`

defined in Algorithm 1 is used to check if the time-derivative of a set-based task *σ* with a valid set *C* = [*σ*_{min}, *σ*_{max}] is in the modified tangent cone of *C*, i.e., if $\dot{\mathrm{\sigma}}\in {T}_{\mathbb{R},C}\left(\mathrm{\sigma}\right)$. The algorithm `in_T_RC`

is illustrated in Figure 2.

**Figure 2. Graphic illustration of the function in_T_RC with return value True shown in green and False in red**. The function only returns

`False`

when *σ*is outside or on the border of its valid set and the derivative points away from the valid set.

Note that in the implementation of the algorithm, `in_T_RC`

is used as a check both for the high-priority and low-priority set-based task. This is to handle small numerical inaccuracies that result from discretization of a continuous system. Furthermore, given initial conditions outside the valid set *C*, the chosen implementation is still well-defined for all set-based tasks.

For a system with *j* set-based tasks, the 2* ^{j}* modes must be calculated every time step. For practical purposes, it is only necessary to calculate the part of the state vector

*that corresponds to*

**z***, so we calculate this using $\dot{\mathit{\text{q}}}={\mathit{\text{f}}}_{i}\left(\mathit{\text{q}}\right)$ for*

**q***i*∈ {1, …, 2

*}. Then, to decide what mode to activate,*

^{j}`in_T_RC`

is used to check if every inactive set-based task of that mode returns `True`

. If it does, that mode is picked. If not, the next mode is considered. Recall that ${\dot{\mathrm{\sigma}}}_{\text{k}}={\mathit{\text{J}}}_{\text{k}}\dot{\mathit{\text{q}}}$ and denote the *j*th set-based task as

*σ*

_{z}. Define the Boolean variables

*ϕ*

_{i}The active mode is chosen according to Table 3, where the algorithm iterates through priority 1 to 2* ^{j}* and activates the first mode that satisfies the criteria presented in Table 3. The modes are assigned priority according to their restrictiveness. The more set-based tasks are active, the more restrictive it is. Therefore, the least restrictive mode is

**f**_{1}, where no set-based tasks are active. From there,

**f**_{2}−

**f**_{(}

_{j}_{+1)}are the modes where one set-based task is active, etc. In Table 3, a hyphen indicates that the Boolean value does not need to be checked because in that mode, the corresponding set-based task is active and therefore satisfied by definition (in case of a high-priority set-based task) or actively handled to the best of the system’s ability (in case of a low-priority set-based task).

**Table 3. Table illustrating the activation of mode based on the Boolean variables as defined in equation (98)**.

### 6.1. Computational Load

Differently from Escande et al. (2014), the proposed algorithm is deterministic from the computational load aspect. It is possible, in fact, to provide an estimate with respect to the main parameters of the system. In particular, it is seen that the most critical variable for the computational load is the number of DOFs of the system.

The cost of a certain operation is denoted as *c*(⋅), the following assumptions have been done:

• sum and subtraction have zero cost,

• the cost of matrix multiplication between a *m*× *p* and a *p* × *n* is proportional to *mpn*,

• the cost of matrix inversion of a *n* × *n* is proportional to *n*^{3}.

Denoting the number of DOFs of the system as *n* and the generic task dimension as *m*_{x}, the following holds:

By considering two generic a and b tasks, each projection within the null-space costs:

In the proposed method, this computational cost is present as many times as there are equality plus active set-based tasks. However, it is easy to recognize that this cost is *small* for the higher-priority task and grows when the lowest priority are projected into the null space of the stacked Jacobian of all the higher priorities. The worst case, from the computational aspect, arises thus when *m*_{a} or *m*_{b} are as large as possible, i.e., *n* − 1, in which case

By assuming that all the DOFs are exploited, it is possible to verify that this cost arises both in the classical equality-task-priority inverse kinematics and in the extension to set-based tasks presented here.

The recursive computation of the null spaces given in Antonelli et al. (2015) does not change the dependence of the cost with respect to the number of DOFs of the system, since it implies a matrix multiplication between two *n* × *n* matrices whose cost still is proportional to *n*^{3}.

Overall, the cost is proportional to *n*^{3}.

One additional computation cost arising for the set-based extension in this paper is the check whether or not one of the possible solutions is violating one of the set-based tasks. This implies re-projection of ${\dot{\mathit{\text{q}}}}_{\text{des}}$ by means of **J**_{x} at an individual cost of *nm*_{x}. However, we only consider scalar set-based tasks, for which *m*_{x} = 1 and the cost is proportional to *n*.

Furthermore, for *j* set-based tasks, in the worst-case scenario, it is necessary to compute 2* ^{j}* possible solutions. However, in many cases, some of the 2

*modes have common terms, which will reduce the overall cost, since these terms only need to be computed once. Furthermore, simulations and experimental results confirm that the algorithm is feasible and sufficiently fast for real-time applications.*

^{j}It is also worth noticing that optimized algorithms for matrix inversion exist but are not considered here. In fact, optimized methods usually exhibit different performances based on the input, and in general, they have a smaller cost than the costs considered here.

## 7. Experimental Results

This section presents experimental results that were obtained by running the proposed algorithm on a 6 DOF manipulator from Universal Robots called UR5. Two examples are presented that illustrate the implementation and effectiveness of the proposed method.

### 7.1. UR5 and Control Setup

The UR5 is a manipulator with 6 revolute joints, and the joint angles are denoted $\mathit{\text{q}}\triangleq {\left[\begin{array}{cccccc}\hfill {q}_{1}\hfill & \hfill {q}_{2}\hfill & \hfill {q}_{3}\hfill & \hfill {q}_{4}\hfill & \hfill {q}_{5}\hfill & \hfill {q}_{6}\hfill \end{array}\right]}^{\text{T}}$. In this paper, the Denavit–Hartenberg (D–H) parameters are used to calculate the forward kinematics. The parameters are given in Wu et al. (2014) and are presented Table 4 with the corresponding coordinate frames illustrated in Figure 3. The resulting forward kinematics has been experimentally verified to confirm the correctness of the parameters.

**Figure 3. Coordinate frames corresponding to the D–H parameters in Table 4**.

The UR5 is equipped with a high-level controller that can control the robot both in joint and Cartesian space. In the experiments presented here, a calculated reference **q**_{des} is sent to the high-level controller, which is assumed to function nominally such that

From this reference, ${\dot{\mathit{\text{q}}}}_{\text{des}}$ and ${\ddot{\mathit{\text{q}}}}_{\text{des}}$ are extrapolated and sent with **q**_{des} to the low-level controller.

The structure of the system is illustrated in Figure 4. The algorithm described in Section 6 is implemented in the kinematic controller block. For every time step, a reference for the joint velocities is calculated and integrated to desired joint angles **q**_{des}. This is used as input to the dynamic controller, which in turn applies torques to the joint motors. Note that the actual state * q* is not used for feedback to the kinematic control block. When the current state is used as input for the kinematic controller, the kinematic and dynamic loops are coupled and the gains designed for the kinematic control alone to satisfy Assumption 1 can not be used. This result in uneven motion, and therefore the kinematic control block receives the previous reference as feedback, which leads to much nicer behavior, and is a good approximation because the dynamic controller tracks the reference with very high precision. This is a standard method of implementation for industry robots when kinematic control is used.

**Figure 4. The control structure of the experiments**. The tested algorithm is implemented in the kinematic controller block.

The communication between the implemented algorithm and the industrial manipulator system occurs through a TCP/IP connection, which operates at 125 Hz. The kinematic control block is implemented using python, which is a very suitable programing language for the task. The TCP/IP connection is very simple to set up in python. Furthermore, python has several libraries that can handle different math and matrix operations.

### 7.2. Implemented Tasks

Three tasks make up the basis for the experiments: position control, collision avoidance, and field of view (FOV). Position control is implemented as both an equality and set-based task and collision avoidance and FOV as set-based tasks.

#### 7.2.1. Position Control

The position of the end-effector relative to the base coordinate frame is given by the forward kinematics. The analytical expression can be found through the homogeneous transformation matrix (Spong et al., 2005) using the D–H parameters given in Table 4. The task is then defined by

where the function * f* (

*) is given by the forward kinematics.*

**q**#### 7.2.2. Collision Avoidance

To avoid a collision between the end effector and an object at position **p**_{o} ∈ ℝ^{3}, the distance between them is used as a task:

#### 7.2.3. Field of View

The field of view is defined as the outgoing vector of the end effector, i.e., the *z*_{6}-axis in Figure 3. This vector expressed in base coordinates is denoted *a* ∈ ℝ^{3} and can be found through the homogeneous transformation matrix using the D–H parameters:

FOV is a useful task when directional devices or sensors are mounted on the end effector, and they are desired to point in a certain direction *a*_{des} ∈ ℝ^{3}. The task is defined as the norm of the error between * a* and

**a**_{des}:

Note in equations (108) and (112) that **J**_{CA} and **J**_{FOV} are not defined for σ_{CA} = 0 and σ_{FOA} = 0, respectively. In the implementation, this is solved by adding a small ε > 0 to the denominator of these two Jacobians, thereby ensuring that division by zero does not occur. Furthermore, the method presented in this paper ensures collision avoidance; hence, *σ*_{CA} will never be zero. Also, note that alternative FOV functions exist which do not suffer from this representation singularity.

### 7.3. Example 1

In this example, the system has been given two waypoints for the end effector to reach. This is the sole equality task *σ*_{1} of this example:

A circle of acceptance (COA) of 0.02 m is implemented for switching from **σ**_{1,des} = **σ**_{pos,des} = **p**_{w1} to **σ**_{1,des} = **σ**_{pos,des} = **p**_{w2}. The task gain matrix has been chosen as

Furthermore, two obstacles have been introduced; hence, two collision avoidance tasks are necessary. In this example, these tasks are high-priority and are denoted *σ*_{a} and *σ*_{b}, respectively. The obstacles are positioned at

and have a radius of 0.18 and 0.15 m, respectively. This radius is used as the minimum value of the set-based collision avoidance task to ensure that the end effector is never closer to the obstacle center than the allowed radius, see Table 5. Because the task is only considered as a high-priority set-based task, it is not necessary to choose a task gain.

FOV is implemented as a low-priority set-based task σ_{c} = σ_{FOV}, with a maximum value to limit the error between * a* and

**a**_{des}. Here, the maximum value for the set-based FOV task is set as 0.2622. This corresponds to allowing the angle between

**a**_{des}and

*being 15° or less. If the FOV error exceeds, the maximum value of 0.2622, rather than attempting to freeze the task at its current value, an effort is made to push it back to the boundary of the valid set:*

**a**with

The gain for this task is **Λ**_{FOV} = 1. The priority of the tasks is given below in the table below.

A system with 3 set-based tasks normally have 2^{3} = 8 modes to consider. However, in this case, the two obstacles have no points of intersection. Hence, it will never be necessary to freeze both *σ*_{a} and *σ*_{b} simultaneously, and thus the system has 6 modes:

Denote the Boolean variables

The active mode is then chosen by Table 6.

The results of Example 1 are shown in Figure 5. The position task is fulfilled as predicted by the theory, i.e., the two waypoints are reached by the end effector. Figures 5A,B confirm this. In Figure 5B, it can be seen that the error between the actual and desired end position converges toward zero. Then, at around *t* = 12 s, the end effector is within the circle of acceptance of **p**_{w1}. Thus, the desired position switches to **p**_{w2}. The end effector immediately starts moving toward the new waypoint and converges to it at around *t* = 35 s. Furthermore, the end effector avoids the two obstacles by locking the distance to the obstacle center at the obstacle radius until the other active tasks drive the end effector away from the obstacle center on their own accord. This can be seen in Figure 5A, and it is also confirmed by Figure 5D: the set-based collision avoidance tasks never exceed the valid sets *C*_{a} and *C*_{b}, but freeze on the boundary of these sets.

**Figure 5. Logged data from Example 1. (A)** End-effector trajectory. The two waypoints plotted in red. The figure shows that the end-effector moves around the obstacles to reach its desired position. **(B)** Position error ${\tilde{\mathrm{\sigma}}}_{1}$ in *x*, *y*, and *z*-direction. **(C)** Active mode over time. **(D)** End effector distance to the two obstacle centers. **(E)** Field of view task.

Figure 5C displays the active mode over time and confirms that mode changes coincide with set-based tasks either being activated (frozen on boundary/leaving valid set) or deactivated (unfrozen/approaching valid set). An increase in mode means that a new set-based task has been activated and vice versa.

Even though the initial condition of *σ*_{c} is outside *C*_{c}, the task is not active from *t* = 0 s because other, less restrictive modes, naturally bring the task closer to and eventually into *C*_{c}. However, at around *t* = 13 s, the system enters mode 4 and activates *σ*_{c} because mode 1–3 would all lead to *σ*_{c} leaving *C*_{c}. Due to the fact that *σ*_{c} is a low-priority set-based task, it still exceeds its maximum value in spite of the system activating the task, as predicted by the theory. However, by keeping the task active, eventually *σ*_{c} converges back to the boundary of *C*_{c}.

### 7.4. Example 2

In this example, the system has been given a time-varying trajectory for the end effector to track:

The task gain matrix has been chosen as

In addition, the system is also given a valid workspace: a minimum and maximum value for the *x*, *y* and *z* position of the end effector. This is implemented as three set-based tasks that are equal to the first, second, and third element of **σ**_{pos}, respectively. These set-based tasks are all implemented as high-priority, as shown in the Table 7 below.

A system with 3 set-based tasks has 2^{3} = 8 modes to consider:

Denote the Boolean variables

The active mode is then chosen according to Table 8.

The results of Example 2 are shown in Figures 6 and 7. The system is unable to track the equality task perfectly, as the desired trajectory moves in and out of the allowed workspace (Figures 6A and 7). Mathematically, this is explained by the fact that the set-based tasks and the position equality task are not linearly independent (in fact, they are equal to each other). Thus, Assumption 1 is not fulfilled, and the equality task errors can no longer be guaranteed to converge to zero. However, as seen in Figure 6C, the end effector stays within the valid workspace at all times by freezing on the boundary when following the trajectory would bring it outside of the valid workspace. Figure 6B displays the active mode over time. Mode changes clearly correspond to set-based tasks being activated/deactivated on the border of their valid sets.

**Figure 6. Logged data from example 2. (A)** Desired and actual end trajectory plotted in red and blue, respectively. The allowed workspace is illustrated by the green box. Initial and end position marked by *x* and *o*, respectively. **(B)** Active mode over time. **(C)** Reference and actual end effector position in *x*, *y*, and *z*-direction with valid workspace limits.

**Figure 7. Actual vs. desired end effector trajectory from example 1 plotted in the xy-, xz-, and yz-plane, respectively**. A three-dimensional plot is given in Figure 6A.

Videos of the experimental results can be viewed online.^{1}

## 8. Conclusion

A general robotic system is controlled in joint space but has desired behavior (tasks) specified in task space. The singularity-robust multiple task-priority inverse kinematics framework has been developed as a way to generate a reference trajectory in joint space to fulfill several tasks in a prioritized order simultaneously. This framework has been developed for *equality tasks*, which are tasks that assign an exact desired value to the controlled variable (e.g., the end-effector configuration). However, for a general robotic system, several goals may be described as *set-based tasks*, i.e., tasks with a desired interval [*area of satisfaction* (Escande et al., 2014)]. Two classic, yet important examples are joint limits and obstacle avoidance. Additional examples are manipulability, dexterity, and field of view (for directional sensors, such as a camera mounted on the end effector).

This paper presents an extension to the singularity-robust multiple task-priority inverse kinematics framework that enables set-based tasks to be handled directly. The proposed method allows a general number of scalar set-based tasks to be handled with a given priority within a number of equality tasks. The main purpose of this method is to fulfill the system’s equality tasks while ensuring that the set-based tasks are always satisfied, i.e., contained in their valid set. A mathematical framework is presented and concluded with a stability analysis, in which it is proven that high-priority set-based tasks remain in their valid set at all times, whereas lower-priority set-based tasks cannot be guaranteed to be satisfied due to the influence of the higher-priority equality tasks. Furthermore, it is proven that the equality task errors converge asymptotically to zero given certain assumptions.

A practical implementation of the proposed algorithm is presented along with an analysis of the computational cost. Finally, experimental results are presented where two examples have been implemented on a UR5 manipulator. The experimental results confirm the theory and prove the validity of the proposed method for practical purposes.

Future work includes expanding the algorithm to achieve smooth transitions between switches while maintaining the strict priority of all tasks and still guaranteeing satisfaction of the high-priority set-based tasks.

## Author Contributions

SM: writing the paper, developing the theory, and running experiments. GA: writing the paper, developing the theory, providing feedback to theoretical and experimental results, and feedback on written paper. ART: developing the theory and feedback on written paper. KYP: developing the theory, providing feedback to theoretical and experimental results, and feedback on written paper. JS: running experiments and feedback on written paper.

## Conflict of Interest Statement

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

## Funding

This work was supported by the Research Council of Norway through the Center of Excellence funding scheme, project number 223254, by the Italian *Ministero dell’Istruzione*, *dell’Università e della Ricerca*, *PRIN 2010–2011* through the project MARIS (protocol 2010FBLHRJ) and by the European Community through the projects ARCAS (FP7-287617), EuRoC (FP7-608849), DexROV (H2020-635491), and AEROARMS (H2020-644271).

## Footnote

## References

Antonelli, G. (2008). “Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems,” in *Proc. 2008 IEEE International Conference on Robotics and Automation* (Pasadena, CA: IEEE), 1993–1998.

Antonelli, G. (2009). Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems. *IEEE Trans. Robot.* 25, 985–994. doi: 10.1109/TRO.2009.2017135

Antonelli, G. (2014). *Underwater Robots*, 3rd Edn. Heidelberg: Springer Tracts in Advanced Robotics, Springer-Verlag.

Antonelli, G., Arrichiello, F., and Chiaverini, S. (2008). The null-space-based behavioral control for autonomous robotic systems. *J. Intell. Serv. Robot.* 1, 27–39. doi:10.1007/s11370-007-0002-3

Antonelli, G., Moe, S., and Pettersen, K. (2015). “Incorporating set-based control within the singularity-robust multiple task-priority inverse kinematics,” in *Proc. 23th Mediterranean Conference on Control and Automation* (Torremolinos: IEEE), 1132–1137.

Azimian, H., Looi, T., and Drake, J. (2014). “Closed-loop inverse kinematics under inequality constraints: application to concentric-tube manipulators,” in *Proc. 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2014)* (Chicago, IL: IEEE), 498–503.

Baizid, K., Giglio, G., Pierri, F., Trujillo, M., Antonelli, G., Caccavale, F., et al. (2015). “Experiments on behavioral coordinated control of an unmanned aerial vehicle manipulator system,” in *IEEE International Conference on Robotics and Automation (ICRA)* (Seattle, WA: IEEE), 4680–4685.

Buss, S. R. (2009). *Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares Methods*. San Diego, CA: University of California. Available at: https://www.math.ucsd.edu/∼sbuss/ResearchWeb/ikmethods/iksurvey.pdf

Chiaverini, S. (1997). Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators. *IEEE Trans. Robot. Autom.* 13, 398–410. doi:10.1109/70.585902

Chiaverini, S., Oriolo, G., and Walker, I. D. (2008). “Kinematically redundant manipulators,” in *Springer Handbook of Robotics*, eds B. Siciliano and O. Khatib (Heidelberg: Springer-Verlag), 245–268.

de Lasa, M., Mordatch, I., and Hertzmann, A. (2010). Feature-based locomotion controllers. *ACM Trans. Graph.* 29, 131:1–131:10. doi:10.1145/1778765.1781157

Escande, A., Mansard, N., and Wieber, P.-B. (2014). Hierarchical quadratic programming: fast online humanoid-robot motion generation. *Int. J. Robot. Res.* 33, 1006–1028. doi:10.1177/0278364914521306

Faverjon, B., and Tournassoud, P. (1987). “A local based approach for path planning of manipulators with a high number of degrees of freedom,” in *Proc. 1987 IEEE International Conference on Robotics and Automation (ICRA)*, Vol. 4 (Raleigh, NC: IEEE), 1152–1159.

Goebel, R., Sanfelice, R., and Teel, A. (2012). *Hybrid Dynamical Systems: Modeling, Stability, and Robustness*. Princeton, NJ: Princeton University Press.

Golub, G., and Van Loan, C. (1996). *Matrix Computations*, 3rd Edn. Baltimore, MD: The Johns Hopkins University Press.

Ioannou, P., and Sun, J. (1996). *Robust Adaptive Control*. Upper River Saddle, NJ: Prentice Hall, Inc.

Kanoun, O., Lamiraux, F., and Wieber, P. (2011). Kinematic control of redundant manipulators: generalizing the task-priority framework to inequality task. *IEEE Trans. Robot.* 27, 785–792. doi:10.1109/TRO.2011.2142450

Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. *Int. J. Robot. Res.* 5, 90. doi:10.1177/027836498600500106

Liégeois, A. (1977). Automatic supervisory control of the configuration and behavior of multibody mechanisms. *IEEE Trans. Syst. Man Cybern.* 7, 868–871. doi:10.1109/TSMC.1977.4309644

Maciejewski, A., and Klein, C. (1985). Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments. *Int. J. Robot. Res.* 4, 109–117. doi:10.1177/027836498500400308

Mansard, N., and Chaumette, F. (2007). Task sequencing for high-level sensor- based control. *IEEE Trans. Robot. Autom.* 23, 60–72. doi:10.1109/TRO.2006.889487

Mansard, N., Khatib, O., and Kheddar, A. (2009). A unified approach to integrate unilateral constraints in the stack of tasks. *IEEE Trans. Robot.* 25, 670–685. doi:10.1109/TRO.2009.2020345

Moe, S., Antonelli, G., Pettersen, K. Y., and Schrimpf, J. (2015a). “Experimental results for set-based control within the singularity-robust multiple task-priority inverse kinematics framework,” in *Proc. 2015 IEEE International Conference on Robotics and Biomimetics (ROBIO 2015)* (Zhuhai).

Moe, S., Teel, A., Antonelli, G., and Pettersen, K. (2015b). “Stability analysis for set-based control within the singularity-robust multiple task-priority inverse kinematics framework,” in *Proc. 54th IEEE Conference on Decision and Control* (Osaka).

Nakamura, Y., Hanafusa, H., and Yoshikawa, T. (1987). Task-priority based redundancy control of robot manipulators. *Int. J. Robot. Res.* 6, 3–15. doi:10.1177/027836498700600103

Siciliano, B. (1990). Kinematic control of redundant robot manipulators: a tutorial. *J. Intell. Robot. Syst.* 3, 201–212. doi:10.1007/BF00126069

Siciliano, B., Sciavicco, L., Villani, L., and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer-Verlag.

Siciliano, B., and Slotine, J.-J. (1991). “A general framework for managing multiple tasks in highly redundant robotic systems,” in *Proc. 5th International Conference on Advanced Robotics* (Pisa: IEEE), 1211–1216.

Simetti, E., Casalino, G., Torelli, S., Sperindé, A., and Turetta, A. (2013). Floating underwater manipulation: developed control methodology and experimental validation within the TRIDENT project. *J. Field Robot.* 31, 364–385. doi:10.1002/rob.21497

Spong, M., Hutchinson, S., and Vidyasagar, M. (2005). *Robot Modeling and Control*. Hoboken, NJ: Wiley.

Whitney, D. (1969). Resolved motion rate control of manipulators and human prostheses. *IEEE Trans. Man Mach. Syst.* 10, 47–52. doi:10.1109/TMMS.1969.299896

Keywords: robotics, kinematic control, set-based control, task-priority, redundant systems

Citation: Moe S, Antonelli G, Teel AR, Pettersen KY and Schrimpf J (2016) Set-Based Tasks within the Singularity-Robust Multiple Task-Priority Inverse Kinematics Framework: General Formulation, Stability Analysis, and Experimental Results. *Front. Robot. AI* 3:16. doi: 10.3389/frobt.2016.00016

Received: 07 December 2015; Accepted: 22 March 2016;

Published: 18 April 2016

Edited by:

Zoe Doulgeri, Aristotle University of Thessaloniki, GreeceReviewed by:

Dongbin Lee, Oregon Institute of Technology, USACharalampos P. Bechlioulis, National Technical University of Athens, Greece

Copyright: © 2016 Moe, Antonelli, Teel, Pettersen and Schrimpf. This is an open-access article distributed under the terms of the Creative Commons Attribution License (CC BY). The use, distribution or reproduction in other forums is permitted, provided the original author(s) or licensor are credited and that the original publication in this journal is cited, in accordance with accepted academic practice. No use, distribution or reproduction is permitted which does not comply with these terms.

*Correspondence: Signe Moe, signe.moe@itk.ntnu.no