Experimental Results for Set-based Control within theSingularity-robust Multiple Task-priority Inverse Kinematics

Framework

Signe Moe1, Gianluca Antonelli2, Kristin Y. Pettersen1 and Johannes Schrimpf3

Abstract— Inverse kinematics algorithms are commonly usedin robotic systems to achieve desired behavior, and severalmethods exist to ensure the achievement of numerous taskssimultaneously. The multiple task-priority inverse kinematicsframework allows a consideration of tasks in a prioritized orderby projecting task velocities through the null-spaces of higherpriority tasks. Recent results have extended this frameworkfrom equality tasks to also handling set-based tasks, i.e. tasksthat have an interval of valid values. The purpose of thispaper is to further investigate and experimentally validate thisalgorithm and its properties. In particular, this paper presentsexperimental results where a number of both set-based andequality tasks have been implemented on the 6 Degree ofFreedom UR5 which is an industrial robotic arm from UniversalRobots. The experiments validate the theoretical results.

I. INTRODUCTION

Robotic systems with a large number of Degrees ofFreedom (DOFs) are frequently used for industrial purposes[1] and are becoming increasingly important within a varietyof fields, including unmanned vehicles such as underwater[2] and aerial [3] systems.

Traditionally, robotic systems are controlled in their jointspace. However, the tasks they are required to execute areoften defined in the operational space, for instance givenby the desired end effector position or orientation. On akinematic level, the most common approach is to use aJacobian-based method as a mapping from operational tojoint space [4]-[6]. In particular, the pseudo-inverse Jacobianis defined for systems that are not square or have full rank,and is a widely used solution to the inverse kinematicsproblem [7]-[9].

A robotic system is defined as kinematically redundantif it possesses more DOFs than those required to performa certain task [10]. In this case, the ”surplus” DOFs canbe employed to achieve several tasks using Null-Space-Based (NSB) behavioral control, also known as multipletask-priority inverse kinematics [11]. This framework, which

1S.Moe and K.Y.Pettersen are with the Center for AutonomousMarine Operations and Systems (AMOS), at The Departmentof Engineering Cybernetics, Norwegian University of Scienceand Technology (NTNU), Trondheim, Norway {signe.moe,kristin.y.pettersen}@itk.ntnu.no

2G.Antonelli is with the Department of Electrical and InformationEngineering, University of Cassino and Southern Lazio, Cassino, Italyantonelli@unicas.it

3J.Schrimpf is with The Department of Engineering Cybernetics, Norwe-gian University of Science and Technology (NTNU), Trondheim, Norwayjohannes.schrimpf@itk.ntnu.no

possesses nice stability qualities [12] and has been success-fully implemented on several robotic systems [13], [14], hasbeen developed for equality tasks, which specify exactlyone desired value for given states of the system. However,for a general robotic system, several objectives may not bedescribed as equality tasks, but as set-based tasks, which aretasks that have a desired set of values rather than one exactdesired value (e.g. staying within joint limits or avoidingobstacles) [15]. As recognized in [16], the multiple task-priority inverse kinematics algorithm is not suitable to handleset-based tasks directly, and these tasks are therefore usuallytransformed into more restrictive equality constraints throughpotential fields or cost functions [17], [18].

A first attempt to systematically include set-based tasks ina prioritized task-regulation framework is proposed in [19]and further improved in [20]. To handle the set-based tasks,the algorithms in [19], [20] transform the inverse kinematicsproblem into a QP problem, and therefore they can notbe utilized directly into the multiple task-priority inversekinematics algorithm. In [21], set-based tasks are handledby resorting to proper activation and regularization functions,but has the limitation that set-based tasks are only consideredwith higher priority than the equality tasks of the system.

A method to include set-based tasks in the NSB frame-work is first introduced in [22], and further formalized andanalyzed in [23]. The result is an on-line kinematic controlalgorithm that generates a real-time reference for the systemsjoint velocities. A set-based task is ignored while the taskvalue is within its valid set, and the remaining tasks ofthe system then decide the trajectory. On the border of thevalid set, the set-based task either remains ignored, or it isimplemented as an equality task with the goal of freezing thetask on the boundary. The proposed algorithm will choosethe latter if the other tasks of the system push the set-basedtask out of its valid set. In the opposite case, the set-basedtask is still ignored. This results in a switched system with2n modes, where n is the number of set-based tasks. Thesystem is proven to achieve asymptotic convergence of allequality tasks and satisfaction of all high-priority set-basedtasks, given that the tasks are linearly independent and thegenerated reference is followed [23]. The purpose of thispaper is to further investigate and experimentally validatethis algorithm and its properties proven in [23]. The set-basedtasks that have been implemented are collision avoidance andfield of view (FOV). Joint limit avoidance, limited workspaceand manipulability are examples of other tasks suitable to be

considered as set-based tasks.The experiments were run on a 6 DOF industrial manipula-

tor from Universal Robots - the UR5. It is equipped with jointservo controllers produced by the robot manufacturer, and isboth highly suitable and commonly used for experimentalverification of control algorithms [24]-[27].

This paper is organized as follows: Section II describesthe UR5, the overall control structure and communicationprotocol used in the experiments. Section III defines thetangent cone of a closed set, which is a crucial part of theimplemented algorithm. The concrete implemented examplesare presented in Section IV, followed by the experimentalresults in Section V. Conclusions are given in Section VI.

In this paper, vectors and matrices are expressed inbold. Furthermore, a task is denoted as σσσ (or σ for one-dimensional tasks). Equality tasks are marked with numbersubscripts and set-based with letters. Furthermore, σσσ =σσσdes−σσσ denotes the task error, i.e. the difference betweenthe desired and actual task value. JJJ†

a is the MoorePenrosepseudoinverse of the Jacobian matrix JJJa, and NNNa , III− JJJ†

aJJJadenotes the corresponding null-space matrix with the prop-erty JJJaNNNa ≡ 000.

II. UR5 AND CONTROL SETUP

A. UR5 Kinematics

The UR5 is a manipulator with 6 revolute joints, and thejoint angles are denoted qqq ,

[q1 q2 q3 q4 q5 q6

]T.In this paper, the Denavit-Hartenberg (D-H) parameters areused to calculate the forward kinematics. The parameters aregiven in Table I and illustrated in Fig. 1.

Joint ai [m] αi [rad] di [m] θi [rad]1 0 π/2 0.089 q12 -0.425 0 0 q23 -0.392 0 0 q34 0 π/2 0.109 q45 0 -π/2 0.095 q56 0 0 0.082 q6

TABLE I: Table of the D-H parameters of the UR5. Thecorresponding coordinate systems can be seen in Fig. 1.

Hand-Eye Calibration and Inverse Kinematics of Robot Arm 583

samples from real robot arm is utilized for performance analysis. Furthermore,this work is implemented as an initial step towards a realtime vision-guidedrobotic manipulation system based on neural network.

The remainder of this paper is organized as follows: The overall platformconsisting of a robot arm and a stereo vision system is described in Section 2.In Section 3, the feedforward neural network is briefly introduced. Then, thetraining of neural networks for calibration and inverse kinematics is presented.In Section 4, the experimental validation and performance comparison are dis-cussed. Finally, conclusions are given in Section 5.

2 System Overview

As mentioned in the previous section, in the present work the neural network isapplied for a 6-DOF robot arm (’hand’) with a stereo vision system (’eye’). Inthis section, these two hardware components will be introduced.

2.1 6-DOF Robot Arm

Universal robot UR5 is a 6-DOF robot arm with relatively lightweight (18.4 kg),see Fig. 2. It consists of six revolute joints which allows a sphere workspacewith a diameter of approximately 170 cm. The Movements close to its boundaryshould be avoided considering the singularity of the arm. The arm is equippedwith a graphic interface PolyScope, which allows users to move the robot in auser-friendly environment through a touch screen. In this work, the connectionto the robot controller is realized at script level using TCP/IP socket. Once the

z0x0

z1

z6z5

z4z3

z2

q1

q2

q3

q4 q5

q6

Fig. 2. UR5 robot and its joint coordinate system. Left : UR5 robot and PolyScopeGUI [15]; Right : joint coordinate system.

Fig. 1: Coordinate frames corresponding to the D-H param-eters in Table I. Illustration from [24].

B. Control Setup and Communication Protocol

The UR5 is equipped with a high-level controller that cancontrol the robot both in joint and Cartesian space. In theexperiments presented here, a calculated reference qqqref is sentto the high-level controller, which is assumed to functionnominally such that

qqq≈ qqqref. (1)From this reference, qqqref and qqqref are extrapolated and sentwith qqqref to the low-level controller.

The structure of the system is illustrated in Fig. 2. The al-gorithm from [23] is implemented in the kinematic controllerblock. Every timestep, a reference for the joint velocitiesis calculated and integrated to desired joint angles qqqref.This is used as feedback to close the kinematic loop andas input to the dynamic controller, which in turn appliestorques to the joint motors. The communication betweenthe implemented algorithm and the industrial manipulatorsystem occurs through a TCP/IP connection which operatesat 125 Hz. The algorithm itself is implemented in Python.

Kinema'c)Control)

Equality)and))set.based)tasks)

Dynamic)Control)

Robot)manipulator)

Industrial)manipulator)system)

)

∫))

q,)q,)q).)))..)qref)qref).)))Desired)values)

for)eq.)tasks)Boundary)values)for)set.based)tasks)

Γ)

Fig. 2: The control structure of the experiments. The testedalgorithm is implemented in the kinematic controller block.

III. TANGENT CONE

This section presents the tangent cone of a closed set,which is used in the implementation to decide which set-based tasks are activated. For a detailed description of thealgorithm and theoretical background, the interested readeris referred to [23].

The tangent cone of a closed set CC = [a,b] (2)

is defined as

TC(σ) =

[0,∞) σ = aR σ ∈ P

(−∞,0] σ = b(3)

where P is the interior of C.In the implementation, the following boolean function is

used to check if the time-derivative of a set-based task σ

with a valid set C = [σmin,σmax] is in the tangent cone of C,i.e. σ ∈ TC(σ):

bool in_T_C(sigma_dot,sigma,sigma_min,sigma_max)if sigma > sigma_min and sigma < sigma_max

return Trueelse if sigma <= sigma_min and sigma_dot >= 0

return Trueelse if sigma <= sigma_min and sigma_dot < 0

return Falseelse if sigma >= sigma_max and sigma_dot <= 0

return Trueelse if sigma >= sigma_max and sigma_dot > 0

return False

in_T_C is illustrated in Fig. 3.

σmin$$

σmax$$

σ$

σ$>$0$

σ$<$0$

σ$=$0$.$

.$

.$

Fig. 3: Graphic illustration of in_T_C with return valueTrue shown in green and False in red. The function onlyreturns False when σ is outside or on the border of itsvalid set and the derivative points away from the valid set.

Note that in the implementation of the tangent cone, theborder is not implemented as a perfect equality as in thedefinition (3), but as an inequality (i.e. σ ≤ σmin rather thanσ = σmin). This is to handle small numerical inaccuraciesthat result from discretization of a the continuous system.Furthermore, given initial conditions outside the valid set C,the chosen implementation is still well-founded.

IV. IMPLEMENTED EXAMPLES

In this section, three individual implemented tasks (po-sition control, collision avoidance and field of view) aredefined and numeric values are given for their desiredvalue/valid set (Section IV-A). These tasks have then beenimplemented in three examples with different combinationsof task priority and set-based/equality tasks. The examplesare described in Sections IV-B to IV-D.

A. Implemented Tasks

Three tasks make up the basis for the experiments: Posi-tion control, collision avoidance, and FOV. In the examples,position control is always implemented as an equality taskand collision avoidance as a set-based task. FOV has beenimplemented as both an equality (Example 1) and a set-based(Examples 2 and 3) task.

1) Position Control: The position of the end effectorrelative to the base coordinate frame is given by the forwardkinematics. The analytical expression can be found throughthe homogeneous transformation matrix using the D-H pa-rameters given in Table I.

σσσpos = fff (qqq) ∈ R3 (4)

σσσpos = JJJpos(qqq)qqq =d fffdqqq

qqq (5)

In these experiments, the system has been given twowaypoints for the end effector to reach:

pppw1 =[0.486 m −0.066 m −0.250 m

]T,

and (6)

pppw2 =[0.320 m 0.370 m −0.250 m

]T.

(7)A circle of acceptance (COA) of 0.02 m is implemented

for switching from σσσpos,des = pppw1 to σσσpos,des = pppw2. The taskgain matrix has been chosen as

ΛΛΛpos = diag([0.3 0.3 0.3

]). (8)

This relatively low task gain it to ensure that the positiontask does not ask for too great joint velocities even whenthe task error is large, which would cause the UR5 to entera security stop mode.

2) Collision Avoidance: To avoid a collision between theend effector and an object at position pppo ∈ R3, the distancebetween them is used as a task:

σCA =√(pppo−σσσpos)T(pppo−σσσpos) ∈ R (9)

σCA = JJJCA(qqq)qqq =−(pppo−σσσpos)

T

σCAJJJpos(qqq)qqq (10)

In all experiments, two obstacles have been introduced,hence two collision avoidance tasks are necessary. Theobstacles are positioned at

pppo1 =[0.40 m −0.25 m −0.33 m

]T and (11)

pppo2 =[0.40 m 0.15 m −0.33 m

]T,

(12)and have a radius of 0.18 m and 0.15 m, respectively. Thisradius is used as the minimum value of the set-based collisionavoidance task to ensure that the end effector is never closerto the obstacle center than the allowed radius, see Table II-IV. For the collision avoidance task, it is only necessary toensure that the distance between the obstacle and end effectoris greater than some radius to avoid collision, and hence thistask does not have a maximum limit. Because the task isonly considered as a high-priority set-based task, it is notnecessary to choose a task gain.

3) Field of View: The field of view is defined as theoutgoing vector of the end effector, i.e. the z6-axis in Fig. 1.This vector expressed in base coordinates is denoted aaa∈R3,and can be found through the homogeneous transformationmatrix using the D-H parameters.

aaa = ggg(qqq) ∈ R3 (13)

aaa = JJJFOV,3DOF(qqq)qqq =dgggdqqq

qqq (14)

FOV is a useful task when directional devices or sensors aremounted on the end-effector and they are desired to point ina certain direction aaades ∈ R3. In these experiments,

aaades ≡[1 0 0

]T (15)is constant, and the task is defined as the norm of the errorbetween aaa and aaades:

σFOV =√(aaades−aaa)T(aaades−aaa) ∈ R (16)

σFOV = JJJFOV(qqq)qqq =− (aaades−aaa)T

σFOVJJJFOV,3DOF(qqq)qqq (17)

In Example 1, FOV is implemented as an equality task.Since σFOV is defined as the norm of the error, σFOV,des = 0.In Examples 2 and 3, FOV is implemented as a set-basedtask with a maximum value to limit the error between aaaand aaades. Here, the maximum value for the set-based FOVtask is set as 0.2622. This corresponds to allowing the anglebetween aaades and aaa being 15◦ or less. The gain for this taskis chosen as

ΛFOV = 1. (18)

Note in (10) and (17) that JJJCA and JJJFOV are not defined forσCA = 0 and σFOV = 0, respectively. In the implementation,this is solved by adding a small ε > 0 to the denominator ofthese two Jacobians thereby ensuring that division by zerodoes not occur.

B. Example 1

In Example 1, FOV is implemented as an equality task,and the system has two set-based tasks.

Name Task description Type Valid set Cσa Collision avoidance Set-based Ca = [0.18,∞)σb Collision avoidance Set-based Cb = [0.15,∞)σσσ1 Position Equality -σ2 Field of view Equality -

TABLE II: Implemented tasks in Example 1 sorted bydecreasing priority.

According to [23], a system with 2 set-based tasks have22 = 4 modes to consider: One containing only the equalitytasks, one where σa is frozen, one where σb is frozen andone where σa and σb are frozen. However, in this case, thetwo obstacles have no points of intersection. Hence, it willnever be necessary to freeze both σa and σb, and thus thesystem has three modes:

Mode 1: qqqref = fff 1 , JJJ†1ΛΛΛ1σσσ1 +NNN1JJJ†

2Λ2σ2 (19)

Mode 2: qqqref = fff 2 , NNNaJJJ†1ΛΛΛ1σσσ1 +NNNa1JJJ†

2Λ2σ2 (20)

Mode 3: qqqref = fff 3 , NNNbJJJ†1ΛΛΛ1σσσ1 +NNNb1JJJ†

2Λ2σ2 (21)

Using the fact that σa = JJJa fff 1 and σb = JJJb fff 1 in mode 1,the pseudocode below illustrates the implementation of thesystem:

a = in_T_C(J_a*f1,sigma_a,0.18,infty)b = in_T_C(J_b*f1,sigma_b,0.15,infty)

if a==True and b==Truemode = 1q_dot_ref = f1

else if a==Falsemode = 2q_dot_ref = f2

else if b==Falsemode = 3q_dot_ref = f3

C. Example 2

In Example 2, FOV is implemented as a high-priority set-based task, and the system has three set-based tasks in total.

Name Task description Type Valid set Cσa Collision avoidance Set-based Ca = [0.18,∞)σb Collision avoidance Set-based Cb = [0.15,∞)σc Field of view Set-based Cc = (−∞,0.2622]σσσ1 Position Equality -

TABLE III: Implemented tasks in Example 2 sorted bydecreasing priority.

According to [23], this should result in 23 = 8 modes toconsider. However, as in Example 1, we can discard the twomodes where both σa and σb are frozen. Thus, 6 modes haveto be considered:

Mode 1: qqqref = fff 1 , JJJ†1ΛΛΛ1σσσ1 (22)

Mode 2: qqqref = fff 2 , NNNaJJJ†1ΛΛΛ1σσσ1 (23)

Mode 3: qqqref = fff 3 , NNNbJJJ†1ΛΛΛ1σσσ1 (24)

Mode 4: qqqref = fff 4 , NNNcJJJ†1ΛΛΛ1σσσ1 (25)

Mode 5: qqqref = fff 5 , NNNacJJJ†1ΛΛΛ1σσσ1 (26)

Mode 6: qqqref = fff 6 , NNNbcJJJ†1ΛΛΛ1σσσ1 (27)

In mode 2 and 5 σa is frozen, meaning σa ≡ 0. Hence, inmode 2, in_T_C will always return True with σa as input,and it is therefore unnecessary to check this condition. Thesame applies to σb in modes 3 and 6, and σc in modes 4, 5and 6. Thus, the pseudocode of the implementation is givenbelow:a1 = in_T_C(J_a*f1,sigma_a,0.18,infty)b1 = in_T_C(J_b*f1,sigma_b,0.15,infty)c1 = in_T_C(J_c*f1,sigma_c,-infty,0.2622)b2 = in_T_C(J_b*f2,sigma_b,0.15,infty)c2 = in_T_C(J_c*f2,sigma_c,-infty,0.2622)a3 = in_T_C(J_a*f3,sigma_a,0.18,infty)c3 = in_T_C(J_c*f3,sigma_c,-infty,0.2622)a4 = in_T_C(J_a*f4,sigma_a,0.18,infty)b4 = in_T_C(J_b*f4,sigma_b,0.15,infty)b5 = in_T_C(J_b*f5,sigma_b,0.15,infty)a6 = in_T_C(J_a*f6,sigma_a,0.18,infty)

if a1==True and b1==True and c1==Truemode = 1q_dot_ref = f1

else if b2==True and c2==Truemode = 2q_dot_ref = f2

else if a3==True and c3==Truemode = 3q_dot_ref = f3

else if a4==True and b4==Truemode = 4q_dot_ref = f4

else if b5==Truemode = 5q_dot_ref = f5

else if a6==Truemode = 6q_dot_ref = f6

D. Example 3

In Example 3, FOV is implemented as a low-priority set-based task.

Name Task description Type Valid set Cσa Collision avoidance Set-based Ca = [0.18,∞)σb Collision avoidance Set-based Cb = [0.15,∞)σσσ1 Position Equality -σc Field of view Set-based Cc = (−∞,0.2622]

TABLE IV: Implemented tasks in Example 3 sorted bydecreasing priority.

The implementation is very similar to Example 2. How-ever, lower-priority set-based tasks can not be guaranteed tobe satisfied at all times [23]. Hence, if the FOV error exceedsthe maximum value of 0.2622, rather than attempting tofreeze the task at its current value, an effort is made to pushit back to the boundary of the valid set:

σc = σFOV,max−σc = 0.2622−σc (28)

Example 1 Example 2 Example 3

End effectortrajectory.Obstaclesshown asspheres andwaypointsas red dots.

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8−0.5

0

0.5

−0.5

−0.4

−0.3

−0.2

−0.1

0

y [m]x [m]

z[m

]Waypoints pw 1 and pw 2

End effector trajectory σ1(t)

(a)

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8−0.5

0

0.5

−0.5

−0.4

−0.3

−0.2

−0.1

0

y [m]x [m]

z[m

]

Waypoints pw 1 and pw 2

End effector trajectory σ1(t)

(b)

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8−0.5

0

0.5

−0.5

−0.4

−0.3

−0.2

−0.1

0

y [m]x [m]

z[m

]

Waypoints pw 1 and pw 2

End effector trajectory σ1(t)

(c)

Activemode overtime.

0 5 10 15 20 25 30 35 400

1

2

3

4

5

6

7

Time [s]

Mode

Mode

(d)

0 5 10 15 20 25 30 35 400

1

2

3

4

5

6

7

Time [s]

Mode

Mode

(e)

0 5 10 15 20 25 30 35 400

1

2

3

4

5

6

7

Time [s]

Mode

Mode

(f)

Distanceto obstaclecenters overtime.

0 5 10 15 20 25 30 35 400

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

Time [s]

Dista

nce[m

]

σaσ bσa,minσ b ,m in

(g)

0 5 10 15 20 25 30 35 400

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

Time [s]

Dista

nce[m

]

σaσ bσa,minσ b ,m in

(h)

0 5 10 15 20 25 30 35 400

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

Time [s]

Dista

nce[m

]

σaσ bσa,minσ b ,m in

(i)

Norm ofthe errorbetweendesired andactual FOVvectors overtime.

0 5 10 15 20 25 30 35 40−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Time [s]

Norm

ofFOV

errorvecto

r

σ2

(j)

0 5 10 15 20 25 30 35 40−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Time [s]

Norm

ofFOV

errorvecto

r

σ cσ c ,max

(k)

0 5 10 15 20 25 30 35 40−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Time [s]

Norm

ofFOV

errorvecto

r

σ cσ c ,max

(l)

Fig. 4: Logged data from experimental results.

Mode 1: qqqref = fff 1 , JJJ†1ΛΛΛ1σσσ1 (29)

Mode 2: qqqref = fff 2 , NNNaJJJ†1ΛΛΛ1σσσ1 (30)

Mode 3: qqqref = fff 3 , NNNbJJJ†1ΛΛΛ1σσσ1 (31)

Mode 4: qqqref = fff 4 , JJJ†1ΛΛΛ1σσσ1 +NNN1JJJ†

cΛcσc (32)

Mode 5: qqqref = fff 5 , NNNaJJJ†1ΛΛΛ1σσσ1 +NNNa1JJJ†

cΛcσc (33)

Mode 6: qqqref = fff 6 , NNNbJJJ†1ΛΛΛ1σσσ1 +NNNb1JJJ†

cΛcσc (34)

Similarly to Example 2, σa and σb are frozen in modes2 and 5, 3 and 6 respectively. However, since σc is a low-priority set-based task, it can not be guaranteed that it is

frozen on the border of Cc. Therefore, unlike Example 2,in_T_C might return False in modes 4, 5 and 6 withσc as input. Even so, in these modes, an attempt is madeto satisfy the set-based task by actively pushing σc back tothe border of Cc and even if this attempt is unsuccessful(in_T_C = False with σc as input), it is not due to thetask not being handled, but because it is a lower-priority task.Therefore, this condition is not checked in modes 4, 5 and6, and the implementation is identical to Example 2 withmodes defined by (29)-(34).

(a) t = 0 s (b) t = 16 s (c) t = 40 s

Fig. 5: Pictures from simulation and actual experiments, Example 1. In the simulation, the base and end effector coordinatesystem is illustrated with green, blue and red axes for the x-, y- and z-axes respectively. These correspond to the coordinateframes of the actual robot.

V. EXPERIMENTAL RESULTS

This section presents the results from running Examples1-3 on the UR5 manipulator. The relevant logged data isillustrated in Fig. 4, and Fig. 5 displays screenshots/imagesfrom simulation and actual experiment from Example 1.

In all examples, the position task is fulfilled as predictedby the theory [23], i.e. the two waypoints are reached by theend effector. Furthermore, the end effector avoids the twoobstacles by locking the distance to the obstacle center atthe obstacle radius until the other active tasks drive the endeffector away from the obstacle center on their own accord.This can be seen in Figures 4a-4c, and is also confirmed byFigures 4g-4i: The set-based collision avoidance tasks neverexceed the valid sets Ca and Cb, but freeze on the boundaryof these sets.

Figures 4d-4f display the active mode over time, andconfirm that mode changes coincide with set-based taskseither being activated (frozen on boundary/leaving valid set)or deactivated (unfrozen/approaching valid set). An increasein mode means a new set-based task has been activated andvice versa.

In Example 1, FOV is implemented as an equality taskwith lower priority than the position task with the goal ofaligning the FOV vector aaa with aaades =

[1 0 0

]T. Thiscorresponds to the z-axis of the end effector being parallelto the x-axis of the base coordinate system. As can be seen inFig. 4j, the norm of the error between aaa and aaades convergesto zero at about t = 30 s, and Fig. 5 shows that in the endconfiguration, these two vectors are indeed parallel.

In Example 2, FOV is a high-priority set-based task witha maximum value of 0.2622, corresponding to the anglebetween aaa and aaades not exceeding 15◦. The task has initial

conditions outside its valid set Cc (Fig. 4k). However, theother active tasks naturally bring the FOV closer to andeventually (at around t = 4 s) into Cc, and thus it is notnecessary to freeze σc. Once σc enters Cc, the task willalways stay in this set. At around t = 13 s, the system entersmode 4 and σc is frozen because the error between the actualand desired FOV vectors has reached its upper limit andkeeping the task deactivated would result in the maximumvalue being violated. Shortly after, the end effector reachesthe second obstacle, and so mode 6 is activated where bothσb and σc are frozen. Once the end effector has movedaround the obstacle, σb is released. σc, however, can notbe released without leaving Cc, and so the system goes backto mode 4 and remains there for the duration of the example.

In Example 3, FOV is a low-priority set-based task withthe same maximum value as Example 2. By comparingFigures 4k and 4l it is evident that these implementationsbehave similarly until t = 13 s, when the system entersmode 4 and activates σc. In Example 2, the task is frozenon the boundary, which is guaranteed due to the fact it ishigh priority. As explained in Section IV, low priority set-based tasks can not be guaranteed to actually freeze on theboundary, and they are therefore activated with the goal ofpushing the task back to its boundary. This is confirmed byFig. 4l. In this example, σc does indeed exceed its maximumvalue in spite of the system activating the task. However,eventually σc converges back to the boundary of Cc.

Figures 4j and 4l show that implementing FOV as a lowerpriority equality and set-based render similar results. Asexpected, the equality task converges to the exact desiredvalue and the set-based to the boundary of the valid set.Even so, in the case that the system has several other taskswith even lower priority, it might be beneficial to implement

FOV as a set-based task as this imposes less constraint onthe lower-priority tasks when inactive.

VI. CONCLUSIONS

A method proposed in [22], [23] for incorporating set-based tasks in the singularity-robust multiple task-priorityinverse kinematics framework has been illustrated and val-idated in this paper. In particular, the method has beenimplemented on a 6 DOF UR5 manipulator. Three exampleshave been constructed to test various qualities and theperformance of the algorithm. In summary, the experimentalresults confirm the following properties:• All equality tasks converge to their desired value.• All high-priority set-based tasks with initial conditions

in their valid set C stay in this set ∀t ≥ 0.• All high-priority set-based tasks with initial conditions

outside C can only 1) freeze at the current value, or2) move closer to C. Hence, the initial condition is themaximum deviation from C.

• If a high-priority set-based task with initial conditionsoutside its valid set eventually enters C, it will stay inthis set ∀t ≥ te, where te is the time the task entered theset.

• Lower-priority set-based tasks are not necessarily satis-fied.

Furthermore, it is suggested that implementing a low-priority task as set-based rather than as an equality task isless restrictive on even lower priority tasks as they are notaffected by the set-based task when it is not active. Thisremains a topic for future work.

ACKNOWLEDGMENTS

This work was supported by the Research Council ofNorway through the Center of Excellence funding scheme,project number 223254, and by the European Commu-nity through the projects ARCAS (FP7-287617), EuRoC(FP7-608849), DexROV (H2020-635491) and AEROARMS(H2020-644271). The authors would also like to thankMagnus C. Bjerkeng at Sintef ICT, Department of AppliedCybernetics, for sharing advice and experience regardingimplementation on the UR5 manipulator.

REFERENCES

[1] B. Siciliano and O. Khatib, Eds., Springer Handbook of Robotics.Springer Berlin Heidelberg, 2008.

[2] G. Antonelli, Underwater Robots, Third Edition. Springer Interna-tional Publishing, 2014.

[3] K. Valavanis, P. Oh, and L. A. Piegla, Unmanned Aircraft Systems.Springer Netherlands, 2009.

[4] D. Nenchev, Y. Umetani, and K. Yoshida, “Analysis of a redun-dant free-flying spacecraft/manipulator system,” IEEE Transactions onRobotics and Automation, vol. 8, no. 1, pp. 1–6, 1992.

[5] F. Caccavale and B. Siciliano, “Kinematic control of redundant free-floating robotic systems,” Advanced Robotics, vol. 15, no. 4, pp. 429–448, 2001.

[6] O. Egeland and K. Pettersen, “Free-floating robotic systems,” inControl Problems in Robotics and Automation. Springer BerlinHeidelberg, 1998, vol. 230, pp. 119–134.

[7] C. Klein and C.-H. Huang, “Review of pseudoinverse control for usewith kinematically redundant manipulators,” IEEE Transactions onSystems, Man and Cybernetics, vol. 13, no. 2, pp. 245–250, 1983.

[8] S. Chiaverini, G. Oriolo, and I. D. Walker, Springer Handbook ofRobotics. Heidelberg, D: B. Siciliano, O. Khatib, (Eds.), Springer-Verlag, 2008, ch. Kinematically Redundant Manipulators, pp. 245–268.

[9] S. R. Buss, “Introduction to inverse kinematics with jacobian trans-pose, pseudoinverse and damped least squares methods,” IEEE Journalof Robotics and Automation, vol. 17, pp. 1–19, 2004.

[10] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: mod-elling, planning and control. Springer Verlag, 2009.

[11] G. Antonelli, F. Arrichiello, and S. Chiaverini, “The Null-Space-Basedbehavioral control for autonomous robotic systems,” Intelligent ServiceRobotics, vol. 1, no. 1, pp. 27–39, 2008.

[12] G. Antonelli, “Stability analysis for prioritized closed-loop inversekinematic algorithms for redundant robotic systems,” IEEE Transac-tions on Robotics, vol. 25, no. 5, pp. 985–994, October, 2009.

[13] F. Arrichiello, J. Das, H. Heidarsson, S. Chiaverini, and G. Sukhatme,“Experiments in autonomous navigation with an under-actuated sur-face vessel via the null-space based behavioral control,” in Proc.IEEE/ASME International Conference on Advanced Intelligent Mecha-tronics (AIM 2009), July 2009, pp. 362–367.

[14] F. Arrichiello, S. Chiaverini, G. Indiveri, and P. Pedone, “The null-space based behavioral control for a team of cooperative mobile robotswith actuator saturations,” in Proc. IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS 2009), Oct 2009, pp. 5911–5916.

[15] H. Hanafusa, T. Yoshikawa, and Y. Nakamura, “Analysis and controlof articulated robot arms with redundancy,” in Proc. 8th ZFAC WorldCongress, 1981.

[16] O. Kanoun, F. Lamiraux, and P. Wieber, “Kinematic control ofredundant manipulators: Generalizing the task-priority framework toinequality task,” IEEE Transactions on Robotics, vol. 27, no. 4, pp.785–792, 2011.

[17] O. Khatib, “The potential field approach and operational space for-mulation in robot control,” in Adaptive and Learning Systems: Theoryand Applications, K. S. Narendra, Ed. Springer US, 1986.

[18] B. Faverjon and P. Tournassoud, “A local based approach for pathplanning of manipulators with a high number of degrees of freedom,”in Proc. IEEE International Conference on Robotics and Automation,vol. 4, 1987, pp. 1152–1159.

[19] O. Kanoun, F. Lamiraux, and P. Wieber, “Kinematic control ofredundant manipulators: Generalizing the task-priority framework toinequality task,” Robotics, IEEE Transactions on, vol. 27, no. 4, pp.785–792, 2011.

[20] A. Escande, N. Mansard, and P.-B. Wieber, “Hierarchical quadraticprogramming: Fast online humanoid-robot motion generation,” Inter-national Journal of Robotics Research, 2013.

[21] E. Simetti, G. Casalino, S. Torelli, A. Sperinde, and A. Turetta,“Floating Underwater Manipulation: Developed Control Methodologyand Experimental Validation within the TRIDENT Project,” Journalof Field Robotics, vol. 31, no. 3, pp. 364–385, 2013.

[22] G. Antonelli, S. Moe, and K. Pettersen, “Incorporating set-basedcontrol within the singularity-robust multiple task-priority inversekinematics,” in Proc. 23rd Mediterranean Conference on Control andAutomation, Torremolinos, Spain, 2015.

[23] S. Moe, A. Teel, G. Antonelli, and K. Pettersen, “Stability analysis forset-based control within the singularity-robust multiple task-priorityinverse kinematics framework,” in Proc. 54th IEEE Conference onDecision and Control, 2015.

[24] H. Wu, W. Tizzano, T. T. Andersen, N. A. Andersen, and O. Ravn,“Hand-eye calibration and inverse kinematics of robot arm usingneural network,” Advances in Intelligent Systems and Computing, vol.274, pp. 581–591, 2014.

[25] J. J. H. Lee, K. Frey, R. Fitch, and S. Sukkarieh, “Fast path plan-ning for precision weeding,” in Proc. of Australasian Conference onRobotics and Automation, Melbourne, Australia, 2014.

[26] S. Moe and I. Schjolberg, “Real-time hand guiding of industrialmanipulator in 5 dof using microsoft kinect and accelerometer,” inProc. 22th International Symposium on Robot and Human InteractiveCommunication (IEEE RO-MAN), Gyeongju, Korea, 2013, pp. 644–649.

[27] M. Bjerkeng, J. Schrimpf, T. Myhre, and K. Pettersen, “Fast dual-arm manipulation using variable admittance control: Implementationand experimental results,” in Proc. 2014 IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS 2014), Sept 2014,pp. 4728–4734.