Frontiers journals are at the top of citation and impact metrics

Original Research ARTICLE

Front. Robot. AI, 18 April 2016 | https://doi.org/10.3389/frobt.2016.00016

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

imageSigne Moe1*, imageGianluca Antonelli2, imageAndrew R. Teel3, imageKristin Y. Pettersen1 and imageJohannes Schrimpf4
  • 1Center for Autonomous Marine Operations and Systems (AMOS), Department of Engineering Cybernetics, Norwegian University of Science and Technology, Trondheim, Norway
  • 2Department of Electrical and Information Engineering, University of Cassino and Southern Lazio, Cassino, Italy
  • 3Department of Electrical Engineering, University of California Santa Barbara, Santa Barbara, CA, USA
  • 4Department 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 = [q1, q2, …, qn]T. 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) ∈ ℝm as the task variable to be controlled:

σ(t)=f(q(t))

with the corresponding differential relationship:

σ˙(t)=f(q(t))qq˙(t)=J(q(t))q˙(t),

where J(q(t)) ∈ ℝm×n is the configuration-dependent analytical task Jacobian matrix and q˙(t)Rn is the system velocity. For compactness, the argument q of tasks and Jacobians are omitted from the equations in this paper.

Let us first consider a single m-dimensional task to be followed, with a defined desired trajectory σdes(t) ∈ ℝm. The corresponding joint references qdes(t) ∈ ℝn 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:

q˙des=Jσ˙des=JTJJT1σ˙des,

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 (mn) nor have full rank (Buss, 2009):

JJJ=J,
JJJ=J,
(JJ)=JJ,
(JJ)=JJ.

Here, J* denotes the complex conjugate of J.

The vector qdes 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,

q˙des=J(σ˙des+Λσ˜)=Jσ˙ref,

where σ˜Rm is the task error defined as

σ˜=σdesσ

and ΛRm×m is a positive-definite matrix of gains. This feedback approach reduces the error dynamics to

σ˜˙=σ˙desσ˙=σ˙desJq˙=σ˙desJJ(σ˙des+Λσ˜)=Λσ˜,

if q˙=q˙des and J has full rank, implying that JJ = I. Equation (10) describes a linear system with a globally exponentially stable equilibrium point at the equilibrium σ˜=0. It is worth noticing that the assumption q˙=q˙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.

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

q˙des=Jσ˙ref+InJJq˙null,

where In is the (n × n) identity matrix and the vector q˙nullRn is an arbitrary system velocity vector. It can be recognized that the operator (InJ J) projects q˙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.

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:

σ1=f1(q)Rm1
σ2=f2(q)Rm2
σ3=f3(q)Rm3.

For each of the tasks, a corresponding Jacobian matrix can be defined, denoted J1 ∈ ℝm1×n, J2 ∈ ℝm2×n, and J3 ∈ ℝm3×n, respectively. Let us further define the corresponding null-space projectors for the first two tasks as

N1=InJ1J1
N2=InJ2J2.

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

J12A=J1J2

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

N12A=InJ12AJ12A,

where J12A is the pseudoinverse of J12A that satisfies the four Moore–Penrose conditions [equations (4)–(7)]. By expanding the expression for J12A, we see that

J1J2N12A=J1J2InJ1J2J1J2=J1J2J1J2J1J2J1J2=J1J2J1J2=00,

so in general,

JiN12..kA=0fori1,...,k.

The following equation then defines the desired joint velocities:

q˙des=J1σ˙1,refq˙1,des+N1J2σ˙2,refq˙2,des+N12AJ3σ˙3,refq˙3,des

where the definition of σ˙x,ref can be easily extrapolated from equation (8) for each task with the corresponding positive-definite matrix ΛxRmx×mx. The priority of the tasks follows the numerical order, with σ1 being the highest-priority task. Equation (21) also implicitly defines the joint velocities q˙x,desRn 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:

q˙des=J1σ˙1,ref+N1J2σ˙2,ref++N12(k1)AJkσ˙k,ref

where N12(k1)A is the null space of the augmented Jacobian matrix

J12(k1)A=J1J2Jk1.

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) ∈ Dt 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 (σ˙des0). Finally, it is assumed that the desired joint velocities q˙des are tracked perfectly by the system, so that q˙=q˙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
www.frontiersin.org

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

TD(σ)=[0,)σ=σminRσ(σmin,σmax)(,0]σ=σmax.

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 σ˙(t)TD(σ(t))tt0, then this implies that σ(t) ∈ Dtt0. 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 σ˙[0,), then σ will either stay on the border, or move into the interior of the set. Similarly, if σ = σmax and σ˙(,0], σ will not leave D.

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

TR,D(σ)=[0,)σσminRσ(σmin,σmax)(,0]σσmax.

The definition of the extended tangent cone is very similar to the tangent cone, but it is defined for σ ∈ ℝ, not just σD. For σD, σ˙TR,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 2j 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 2jth mode, all set-based tasks are active.

Throughout this section, we consider a robotic system with n DOFs and k equality tasks of mi DOFs each for i ∈ {1, …, k}. Equality task i is denoted σi, the task error is defined as σ˜σi,desσi. Furthermore, the system has j set-based tasks. The first and jth set-based tasks are denoted σa and σx, respectively (where x represents the jth letter of the alphabet). We consider the state-vector z ∈ ℝl, where

l=n+j+i=1kmi,

and

z=qσsbσ˜eb=q1:qnσa:σxσ˜1:σ˜k.

Here, σsb and σ˜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

C:=Rn×C1×C2×...×Cj×Rlnj,

where

C1:=[σa,min,σa,max]:
Cj:=[σx,min,σx,max]

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.,

ρJ12..(i1)A+ρJi=ρJ12..(i1)AJi

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 Λ1=λ1Im1, Λ2=λ2Im2, and Λ3=λ3Im3 for the first, second, and third priority task, respectively, with

λ1>0
λ2>max0,λ¯21λ¯11λ¯22λ1
λ3>max0,λ¯31λ¯11λ¯33λ1,λ¯32λ¯22λ¯33λ2,

where λ¯ij and λ¯ij denote the largest and smallest singular value of the matrix Pij, respectively, and

P11=Im1,P22=J2N1J2,P21=J2J1,P32=J3N1J2,P31=J3J1,P33=J3N12J3.

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 po 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):

σa=(pope)T(pope)
σ˙a=Jaq˙=(pope)T||pope||Jq˙.

Here, pe denotes the position of the end effector and J is the corresponding position Jacobian. For this specific example, we define

C1=[ε,)

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

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:

q˙=J1Λ1σ˜1+N1J2Λ2σ˜2+N12AJ3Λ3σ˜3++N12..(k1)AJkΛkσ˜k
σ˙a=Jaq˙=Ja(J1Λ1σ˜1++N12..(k1)AJkΛkσ˜k)
σ˜˙eb=σ˙eb=σ˙1σ˙2:σ˙k=J1J2:Jkq˙=J1(J1Λ1σ˜1+N1J2Λ2σ˜2++N12..(k1)AJkΛkσ˜k)J2(J1Λ1σ˜1+N1J2Λ2σ˜2++N12..(k1)AJkΛkσ˜k):Jk(J1Λ1σ˜1+N1J2Λ2σ˜2++N12..(k1)AJkΛkσk)=Λ1σ˜1J2J1Λ1σ˜1+J2N1J2Λ2σ˜2:JkJ1Λ1σ˜1+JkN1J2Λ2σ˜2++JkN12..(k1)AJkΛkσk=Λ10m1×m20m1×mkJ2J1Λ1J2N1J2Λ20m2×mk:::JkJ1Λ1JkN1J2Λ2JkN12..(k1)AJkΛkσ˜1σ˜2:σ˜k=M1σ˜eb
z˙=q˙σ˙aσ˜˙ebf11(z)f12(z)f13(z)=f1(z).

The matrix M1 is positive definite by Assumption 1 (Antonelli, 2009).

In mode 1, the set-based task evolves freely according to σ˙a=f12(z), and the time evolution of z should follow the vector field f1(z) as long as the solution z stays in C. If following f1(z) would result in z leaving the set 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 σ˙a=f12(z)<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 σ˜a=σa,desσa0. The system is then defined by the following equations:

q˙=Jaσ˜a+NaJ1Λ1σ˜1+Na1AJ2Λ2σ˜2+Na12AJ3Λ3σ˜3++Na12..(k1)AJkΛkσ˜k=NaJ1Λ1σ˜1+Na1AJ2Λ2σ˜2++Na12..(k1)AJkΛkσ˜k
σ˙a=Jaq˙=Ja(NaJ1Λ1σ˜1+Na12..(k1)AJkΛkσ˜k)=0
σ˜˙eb=σ˙eb=σ˙1σ˙2:σ˙k=J1J2:Jkq˙=J1(NaJ1Λ1σ˜1+Na1AJ2Λ2σ˜2++Na12..(k1)AJkΛkσ˜k)J2(NaJ1Λ1σ˜1+Na1AJ2Λ2σ˜2++Na12..(k1)AJkΛkσ˜k):Jk(NaJ1Λ1σ˜1+Na1AJ2Λ2σ˜2++Na12..(k1)AJkΛkσ˜k)=J1NaJ1Λ1σ˜1J2NaJ1Λ1σ˜1+J2Na1AJ2Λ2σ˜2:JkNaJ1Λ1σ˜1+JkNa1AJ2Λ2σ˜2++JkNa12..(k1)AJkΛkσ˜k=J1NaJ1Λ10m1×m20m1×mkJ2NaJ1Λ1J2Na1AJ2Λ20m2×mk:::JkNaJ1Λ1JkNa1AJ2Λ2JkNa12..(k1)AJkΛkσ˜1σ˜2:σ˜k=M2σ˜eb
z˙=q˙σ˙aσ˜˙ebf21(z)0f23(z)=f2(z).

The matrix M2 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, M2 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 (σ˙a=0).

Let TC(z) denote the tangent cone to C at the point zC:

TC(z)=RlzPRn×[0,)×Rln1z=CP

Consider the continuous functions f1, f2 : C → ℝl as defined in equations (42) and (46). The time evolution of z follows the vector field f1(z) as long as the solution z stays in C (mode 1). Using Lemma 5.26 (Goebel et al., 2012) on the system z˙=f1(z), we know that such a solution exists when f1(x) ∩ TC(x) ≠ ∅ for all x near z (restricting x to C). Hence, we define the set

S:={zC:a neighborhood U ofz:f1(x)TC(x)xCU}.

The discontinuous function f : C → ℝl

f(z):=f1(z)zSf2(z)zCS

then describes our system. The differential equation z˙=f(z) then corresponds to following f1 (mode 1) as long as z stays in C and following f2 (mode 2) otherwise. In mode 2, σa is frozen, so the (n + 1)th element in f2(z) ≡ 0. Consequently, f2(z) ∈ TC(z) ∀zC. This implies that C is strongly forward invariant for z˙=f2(z), i.e., that z(t0) ∈ Cz(t) ∈ Ctt0.

In other words, the set S contains the points z in C such that f1(x) ∈ TC(x) for x in C that are near z. At the border of 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 f1(z), corresponding to σ˙a, must be zero or positive for z to stay in 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 f1(z) will result in σ˙a0, i.e., that the distance between the end effector and obstacle will remain constant or increase.

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 ∈ ℝl 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 i, i ∈ {1, …, n}, then σ = qi and should therefore not be included in the vector σ sb, as it is already included in the state vector through q. With j set-based tasks, it is necessary to consider the 2j solutions resulting from all combinations of activating and deactivating every set-based task. These are presented in Table 1.

TABLE 1
www.frontiersin.org

Table 1. System equations for the resulting 2j modes for j high-priority set-based tasks.

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 Mi for i ∈ {1, …, 2j} are either equal to or a principal submatrix of the general matrix M in equation (60) in Antonelli (2009). Consequently, given Assumption 1, the matrices Mi are positive definite.

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

TC(z)=Rn×TC1(z)×TC2(z)××TCj(z)×Rlnj,

where TCi(z) for i ∈ {1, …, j} is defined as in equation (24). Defining the sets

S1:={zC:aneighborhoodUofz:f1(x)TC(x)xCU}
S2:={zCS1:anbhdUofz:f2(x)TC(x)xCU}
S3:={zC(S1S2):anbhdUofz:f3(x)TC(x)xCU}:
S2j:=CS1S2S3S2j1,

the discontinuous equation z˙=f(z) with f : C → ℝl defined as

f(z):=f1(z)zS1f2(z)zS2f3(z)zS3::f2j(z)zS2j

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 ∈ ℝl and the closed set C, where l, z, and 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 σ˜˙eb=M1σ˜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, σ˜a0 and the joint velocities for mode 2 are given as

q˙=J1Λ1σ˜1+N1JaΛaσ˜a+N1aAJ2Λ2σ˜2++N1a2..(k1)AJkΛkσ˜k=J1Λ1σ˜1+N1aAJ2Λ2σ˜2++N1a2..(k1)AJkΛkσ˜k.

In Section 4.1.1, it was shown that mode 2 implies σ˙a=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):

σ˙a=Jaq˙=Ja(J1Λ1σ˜1+N1aAJ2Λ2σ˜2++N1a2..(k1)AJkΛkσ˜k)=JaJ1Λ1σ˜1

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):

q˙=J1Λ1σ˜1+N1aAJ2Λ2σ˜2++N1a2..(k1)AJkΛkσ˜k
σ˙a=Jaq˙=JaJ1Λ1σ˜1
σ˜˙eb=σ˙eb=σ˙1σ˙2:σ˙k=J1J2:Jkq˙=J1(J1Λ1σ˜1+N1aAJ2Λ2σ˜2++N1a2..(k1)AJkΛkσk)J2(J1Λ1σ˜1+N1aAJ2Λ2σ˜2++N1a2..(k1)AJkΛkσ˜k):Jk(J1Λ1σ˜1+N1aAJ2Λ2σ˜2++N1a2..(k1)AJkΛkσ˜k)=Λ1σ˜1J2J1Λ1σ˜1+J2N1aAJ2Λ2σ˜2:JkJ1Λ1σ˜1+++JkN1a2..(k1)AJkΛkσ˜k=Λ10m1×m20m1×mkJ2J1Λ1J2N1aAJ2Λ20m2×mk:::JkJ1Λ1JkN1aAJ2Λ2JkN1a2..(k1)AJkΛkσ˜1σ˜2:σ˜k
=M2σ˜eb     
z˙=q˙σ˙aσ˜˙ebf21(z)f22(z)f23(z)=f2(z).

Similar to Section 4.1.1, the matrix M2 is positive definite given Assumption 1.

Consider the continuous functions f1, f2: C → ℝl as defined in equations (42) and (62). The time evolution of z should follow the vector field f1(z) as long as the solution z stays in C (mode 1). However, since it cannot be guaranteed that z stays in C, mode 1 should also be active if zC and following f1(z) will result in z keeping its current distance to or moving closer to C. This is mathematically expressed for the system z˙=f1(z) as f1(x)TR,C(x) for all x near z, where

TR,C(z)=Rn×TR,C1(z)×Rln1.

and TR,C1(z) is the extended tangent cone to C1 in equation (29) at the point σa as defined in equation (25). Hence, we define

S:={zRl:a neighborhood U of z:f1(x)TR,C(x)xU}.

The discontinuous function f : C → ℝl

f(z):=f1(z)zSf2(z)zRlS

then describes our system. The differential equation z˙ = f (z) then corresponds to following f1(z) (mode 1) if one of three conditions are satisfied:

(1) σa ∈ (σa,min, σa,max)

(2) σa ≥ σa,max and z˙=f1(z)σ˙a0

(3) σa ≤ σa,min and z˙=f1(z)σ˙a0.

If none of these conditions hold, mode 2 is activated and z˙=f2(z). In mode 2, the set-based task is actively handled by attempting to freeze it, but since the (n + 1)th element in f2(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 ∈ ℝl, where l and z are defined in equations (95) and (27), respectively. We assume that the set-based tasks are labeled so that σb always has lower priority than σa, etc. Furthermore, assume that σa has priority after equality task σpa for some pa = {1, …, k}, σb has priority after equality task σpb for some pb = { pa, …, k}, and so forth. The resulting 2j modes of the system are presented in Table 2.

TABLE 2
www.frontiersin.org

Table 2. System equations for the resulting 2j modes for j low-priority set-based tasks.

For example, for a system with j = 3 set-based tasks and k = 6 equality tasks, pa = 3 and pb = pc = 5, the 2j = 8th mode would be

q˙=J1Λ1σ˜1+N1J2Λ2σ˜2+N12AJ3Λ3σ˜3+N123aAJ4Λ4σ˜4+N123a4AJ5Λ5σ˜5+N123a45bcAJ6Λ6σ˜6

All matrices Mi in Table 2 for i ∈ {1, … 2j} are either equal to or a principal submatrix of the general matrix M in equation (60) in Antonelli (2009). Consequently, given Assumption 1, the matrices Mi are positive definite.

Consider the sets C1-Cj defined by equations (29) and (30) with corresponding extended tangent cones as defined in equation (25). If the ith set-based task is inactive in a given mode f, the corresponding (n + i)th element of vector field f (x) must be in TR,Ci(x) for all x near z. 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 (n + 1) to (n + j) elements of f1(z) must be in their respective extended tangent cones for this mode to be chosen. In mode 2, only σa is active, so in this case all the (n + 2) to (n + j) elements of f2(z) must be in their respective extended tangent cones if mode 2 should be the active mode. Thus, we define

S1:=zRl:a neighborhood U of z:f1(x)Rn×TR,C1(x)××TR,Cj(x)×RlnjxU
S2:=zRlS1:a nbhd U of z:f2(x)Rn×R×TR,C2(x)××TR,Cj(x)×RlnjxU
S3:=zRl(S1S2):a nbhd U of z:f3(x)Rn×TR,C1(x)×R×TR,C3××TR,Cj(x)×RlnjxU:
S2j:=CS1S2S3S2j1.

The discontinuous equation z˙=f(z) with f : C → ℝl defined as

f(z):=f1(z)zS1f2(z)zS2f3(z)zS3::f2j(z)zS2j

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 mi DOFs for i ∈ {1, …, k} and j set-based tasks ∈ ℝ, where jxj of them are high priority and jjx are low-priority at any given priority level. Denote that the jx th, (jx + 1)th, and jth 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 jxth, (jx + 1)th, and jth letter of the alphabet, respectively]. Consider the state-vector z ∈ ℝl, where

l=n+j+i=1kmi,

and

z=qσsbσ˜eb=q1,..,qn,σa,..,σx,σy,..,σz,σ˜1T,..,σ˜kT.T

It can be shown that the 2j 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

σ˜˙eb=Miσ˜eb

for i = {1, …, 2j}. The analysis is similar to the above sections and all matrices Mi are either equal to or a principal submatrix of the general matrix M in equation (60) in Antonelli (2009). Therefore, by Assumption 1, the matrices Mi 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

C1:=[σa,min,σa,max],:
Cjx:=[σx,min,σx,max],
Cjx+1:=[σy,min,σy,max],:
Cj:=[σz,min,σz,max],
C:=Rn×C1××Cjx×Rjjx×Rlnj

and

TC1(z)=[0,)σa=σa,minRσa(σa,min,σa,max)(,0]σa=σa,max,:
TCjx(z)=[0,)σx=σx,minRσx(σx,min,σx,max)(,0]σx=σj,max
TR,Cjx+1(z)=[0,)σyσy,minRσy(σy,min,σy,max)(,0]σyσy,max:
TR,Cj(z)=[0,)σzσz,minRσz(σz,min,σz,max)(,0]σzσz,max.
TCh(z)=TC1(z)×TC2(z)××TCjx(z)
TC(z)=Rn×TCh(z)×Rjjx×Rlnj.

Note that zC 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.

S1:={zC:aneighborhoodUofz:
f1(x)Rn×TCh(x)TCqh(x)×TR,Cjx+1(x)××TR,Cj(x)×RlnjTR,Cjσ(x)  xCUS2:={zC:S1:anbhdUofz:
f2(x)TCqh(x)×TR,Cjx+1(x)××TR,Cjσ(x)xCU:Sjx+1:=zC(S1Sjx):anbhdUofz:
fjx+1(x)TCqh(x)×TR,Cjx+1(x)××TR,Cjσ(x)xCUSjx+2:=zC(S1Sjx+1):anbhdUofz:
fjx+2(x)TCqh(x)×R×TR,Cjx+2(x)××TR,Cjσ(x)xCUSjx+3:=zC(S1Sjx+2):anbhdUofz:
fjx+3(x)TCqh(x)×TR,Cjx+1(x)×R×TR,Cjx+3(x)××TR,Cjσ(x)xCU:S2j:=CS1S2S3S2j1.

The discontinuous equation z˙=f(z) with f : C → ℝl defined as

f(z):=f1(z)zS1f2(z)zS2f3(z)zS3::f2j(z)zS2j

then defines our system. Similar to the analysis of the previous sections, in the sets Si, i ∈ {1, …, 2j}, the elements of the vector flows fi(z) 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.

5. Stability Analysis

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

zCz˙F(z)

where

F(z):=δ>0 cof((z+δB)C)¯zC.

𝔹 is a unit ball in ℝdim(z) centered at the origin. coP¯ 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 2j possible modes of the system.

If V is a continuously differentiable Lyapunov function for z˙=fi(z) for all i ∈ {1, …, 2j}, then V is a Lyapunov function for z˙F(z). The following equation holds as all fi(z) are continuous functions:

fF(z)f=λ1f1(z)+λ2f2(z)++λ2jf2j(z)

for some i=12jλi=1,λi0. Consider the Lyapunov function candidate for the equality task errors:

V(σ˜eb)=12σ˜ebTσ˜eb.

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

V˙=σ˜ebT(λ1(M1σ˜eb)+λ2(M2σ˜eb)++λ2j(M2jσ˜eb))=σ˜ebT(λ1M1+λ2M2++λ2jM2j)σ˜eb=σ˜ebTQσ˜eb.

The convex combination Q of positive-definite matrices is also positive definite. Therefore, V˙ is negative definite and σ˜eb=0 is a globally asymptotically stable equilibrium point in all modes. Thus, the equality task errors σ˜eb asymptotically converge to zero when switching between modes. Furthermore, if q belongs to a compact set, the equilibrium point σ˜eb=0 is exponentially stable.

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 zC, these tasks are not violated.

For a system described by equation (92), z˙=f1(z) as long as the solution zC 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 z to leave C, another mode is activated. If neither of the vector fields f1f2j1 will result in z staying in C, the chosen solution is z˙=f2j(z), for which it has been shown that σ˙a to σ˙x0. Therefore, f2j(z)TC(z)zC, and C is strongly forward invariant for z˙=f2j(z). Thus, there will always exist a solution zC 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 σ˙des0. 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 σ˙TR,C(σ). The algorithm in_T_RC is illustrated in Figure 2.

ALGORITHM 1
www.frontiersin.org

Algorithm 1. The Boolean function in_T_RC.

FIGURE 2
www.frontiersin.org

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 2j modes must be calculated every time step. For practical purposes, it is only necessary to calculate the part of the state vector z that corresponds to q, so we calculate this using q˙=fi(q) for i ∈ {1, …, 2j}. Then, to decide what mode to activate, 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 σ˙k=Jkq˙ and denote the jth set-based task as σz. Define the Boolean variables ϕi

ϕi ≜ in_T_RCJϕfi,σϕ,σϕ,min,σϕ,maxfor i1,,2j and ϕ in a,,z

The active mode is chosen according to Table 3, where the algorithm iterates through priority 1 to 2j 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 f1, where no set-based tasks are active. From there, f2f(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
www.frontiersin.org

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 n3.

Denoting the number of DOFs of the system as n and the generic task dimension as mx, the following holds:

cJx=α1nmx2+α2mx3
cNx=α1nmx2+α2mx3+α3n2mx
cq˙x=α1nmx2+α2mx3+α4nmx

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

cNaq˙b=α1nma2+α2ma3+α3n2ma++α1nmb2+α2mb3+α4nmb+α5n2.

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 ma or mb are as large as possible, i.e., n − 1, in which case

cNaq˙bn3.

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 n3.

Overall, the cost is proportional to n3.

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 q˙des by means of Jx at an individual cost of nmx. However, we only consider scalar set-based tasks, for which mx = 1 and the cost is proportional to n.

Furthermore, for j set-based tasks, in the worst-case scenario, it is necessary to compute 2j possible solutions. However, in many cases, some of the 2j 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.

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 qq1q2q3q4q5q6T. 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.

TABLE 4
www.frontiersin.org

Table 4. Table of the D–H parameters of the UR5.

FIGURE 3
www.frontiersin.org

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 qdes is sent to the high-level controller, which is assumed to function nominally such that

qqdes.

From this reference, q˙des and q¨des are extrapolated and sent with qdes 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 qdes. 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
www.frontiersin.org

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

σpos=f(q)R3
σ˙pos=Jpos(q)q˙=dfdqq˙,

where the function f (q) is given by the forward kinematics.

7.2.2. Collision Avoidance

To avoid a collision between the end effector and an object at position po ∈ ℝ3, the distance between them is used as a task:

σCA=(poσpos)T(poσpos)R
σ˙CA=JCA(q)q˙=(poσpos)TσCAJpos(q)q˙

7.2.3. Field of View

The field of view is defined as the outgoing vector of the end effector, i.e., the z6-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:

a =g(q)R3
a˙=JFOV,3DOF(q)q˙=dgdqq˙

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 ades ∈ ℝ3. The task is defined as the norm of the error between a and ades:

σFOV=(adesa)T(adesa)R
σ˙FOV=JFOV(q)q˙=(adesa)TσFOVJFOV,3DOF(q)q˙

Note in equations (108) and (112) that JCA and JFOV 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:

pw1=[0.486 m0.066 m0.250 m]T, and
pw2=[0.320 m0.370 m0.250 m]T.

A circle of acceptance (COA) of 0.02 m is implemented for switching from σ1,des = σpos,des = pw1 to σ1,des = σpos,des = pw2. The task gain matrix has been chosen as

Λpos=diag(0.30.30.3).

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

po1=[0.40 m0.25 m0.33 m]T, and
po2=[0.40 m0.15 m0.33 m]T,

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.

TABLE 5
www.frontiersin.org

Table 5. Implemented tasks in example 1 sorted by decreasing priority.

FOV is implemented as a low-priority set-based task σc = σFOV, with a maximum value to limit the error between a and ades. Here, the maximum value for the set-based FOV task is set as 0.2622. This corresponds to allowing the angle between ades and a 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:

σ˜c=σFOV,maxσc=0.2622σFOV

with

ades100T.

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 23 = 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:

Mode 1:q˙des=f1J1Λ1σ˜1
Mode 2:σa active,q˙des=f2NaJ1Λ1σ˜1
Mode 3:σb active,q˙des=f3NbJ1Λ1σ˜1
Mode 4:σc active,q˙des=f4J1Λ1σ˜1+N1JcΛcσ˜c
Mode 5:σa,σc active,q˙des=f5NaJ1Λ1σ˜1+Na1AJcΛcσ˜c
Mode 6:σb,σc active,q˙des=f6NbJ1Λ1σ˜1+Nb1AJcΛcσ˜c

Denote the Boolean variables

ϕi ≜ in_T_RC Jϕfi,σϕ,σϕ,min,σϕ,max fori1,,6 and ϕa,,c

The active mode is then chosen by Table 6.

TABLE 6
www.frontiersin.org

Table 6. Table illustrating the activation of mode in example 1.

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 pw1. Thus, the desired position switches to pw2. 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 Ca and Cb, but freeze on the boundary of these sets.

FIGURE 5
www.frontiersin.org

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 σ˜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 Cc, the task is not active from t = 0 s because other, less restrictive modes, naturally bring the task closer to and eventually into Cc. However, at around t = 13 s, the system enters mode 4 and activates σc because mode 1–3 would all lead to σc leaving Cc. 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 Cc.

7.4. Example 2

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

σ1,des(t)=σpos,des(t)=0.5sin2(0.1t)+0.20.5cos(0.1t)+0.25sin(0.1t)0.5sin(0.1t)cos(0.1t)+0.1

The task gain matrix has been chosen as

Λpos=diag([0.150.150.15]).

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.

TABLE 7
www.frontiersin.org

Table 7. Implemented tasks in example 2 sorted by decreasing priority.

A system with 3 set-based tasks has 23 = 8 modes to consider:

Mode 1:q˙des=f1J1Λ1σ˜1
Mode 2:σa active,q˙des=f2NaJ1Λ1σ˜1
Mode 3:σb active,q˙des=f3NbJ1Λ1σ˜1
Mode 4:σc active,q˙des=f4NcJ1Λ1σ˜1
Mode 5:σa,σb active,q˙des=f5NabAJ1Λ1σ˜1
Mode 6:σa,σc active,q˙des=f6NacAJ1Λ1σ˜1
Mode 7:σb,σc active,q˙des=f7NbcAJ1Λ1σ˜1
Mode 8:σa,σb,σc active,q˙des=f8NabcAJ1Λ1σ˜1

Denote the Boolean variables

ϕi ≜ in_T_RCJϕfi,σϕ,σϕ,min,σϕ,maxfor i1,,8 and ϕa,,c

The active mode is then chosen according to Table 8.

TABLE 8
www.frontiersin.org

Table 8. Table illustrating the activation of mode in example 2.

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
www.frontiersin.org

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
www.frontiersin.org

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.

Google Scholar

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

CrossRef Full Text | Google Scholar

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

Google Scholar

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

CrossRef Full Text | Google Scholar

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.

Google Scholar

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.

Google Scholar

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.

Google Scholar

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

Google Scholar

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

CrossRef Full Text | Google Scholar

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.

Google Scholar

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

CrossRef Full Text | Google Scholar

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

CrossRef Full Text | Google Scholar

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.

Google Scholar

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

Google Scholar

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

Google Scholar

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

Google Scholar

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

CrossRef Full Text | Google Scholar

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

CrossRef Full Text | Google Scholar

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

CrossRef Full Text | Google Scholar

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

CrossRef Full Text | Google Scholar

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

CrossRef Full Text | Google Scholar

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

CrossRef Full Text | Google Scholar

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).

Google Scholar

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).

Google Scholar

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

CrossRef Full Text | Google Scholar

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

CrossRef Full Text | Google Scholar

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

Google Scholar

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.

Google Scholar

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

CrossRef Full Text | Google Scholar

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

Google Scholar

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

CrossRef Full Text | Google Scholar

Wu, H., Tizzano, W., Andersen, T. T., Andersen, N. A., and Ravn, O. (2014). Hand-eye calibration and inverse kinematics of robot arm using neural network. Adv. Intell. Syst. Comput. 274, 581–591. doi:10.1007/978-3-319-05582-4_50

CrossRef Full Text | Google Scholar

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, Greece

Reviewed by:

Dongbin Lee, Oregon Institute of Technology, USA
Charalampos 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