Singularity Analysis and Complete Methods to Compute the Inverse Kinematics for a 6-DOF UR/TM-Type Robot

Abstract

Improving the strategies employed to control robotic arms is of great importance because of the increase in their use in advanced supervisory control strategies, such as digital twins. The inverse kinematic (IK) control of manipulators requires an IK solution and an awareness of the singular configurations. This work presents a complete IK calculation system with singularity analysis for the UR5 robotic arm created by Universal Robots. For a specific robot pose, different angle solution sets are obtained, and one of these solution sets has to be selected to achieve movement continuity and avoid singularities. Two methods for this double purpose are proposed: one calculates all the solution possibilities, and the other obtains only one solution set by following a sequence of decisions and calculations clearly stated by a finite state machine (FSM). Both methods are effective in managing singularities. The FSM-based method complements the IK solution procedure with advantages in the number of computations and performance by producing results that would not lead the joints to move abruptly. The results prove that the presented methods select an IK solution that does not result in a singular configuration, and that most of the time, they lead to the same valid IK solution.

1. Introduction

Robots are used to increase production and improve product quality; because of this, Industry 4.0 uses technologies such as robotic manipulators and their digital twins to create cyber–physical systems that are advanced supervisory control systems that improve the overall performance of the physical systems. An inverse kinematic (IK) solution of a robot is required when controlling robots and their digital twins. The twin-in-the-loop architecture [1] is one example of how digital twins can be used. The extensive use of robots and the fact that they interact with their users make it necessary to improve their behavior; this can be achieved by using control strategies such as the IK control and the model reference adaptive control [2]; however, singularities must be avoided for the robot to be free to move in any direction within its workspace and with reasonable joint speed; the latter is necessary because joint velocities tend to infinity as a singular configuration is approached.

Improving the behavior of collaborative robots (cobots) is important because they should safely interact with their users. This is the case of the UR5 robotic arm, a 6-degree-of-freedom (DOF) manipulator created by Universal Robots, and other cobots that have been produced with the same geometry by companies such as Smokie Robotics, Techman Robot, AUBO Robotics, and Doosan Robotics. The kinematics of the UR5 robot has been studied in [3,4,5,6,7,8,9], some solutions have been statistically compared in [10], and a method to classify IK branches of the UR-type robot has been recently proposed [11]. The forward or direct (DK) and inverse kinematics (IK) can be studied with different methods; a well-known technique uses the original or the modified Denavit–Hartenberg (DH) parameters [12]. An IK solution allows for computing the configurations that lead to a specific pose, which is necessary when the position and orientation of the end effector are controlled. Even though works such as [13,14,15,16,17,18] studied the IK of the general 6R serial manipulator that results in 16 solutions, robots with the UR5 geometry only have 8. However, in the analytical and numerical IK solutions of serial robot architectures, it is required to avoid singular configurations; therefore, singularity analysis needs to be considered as a part of the complete IK solution, as in [19]. Although singularity analyses for the UR robot geometry were performed [20,21], a simplified expression is more appropriate for implementations. A code that allows for computing the Jacobian and its determinant can be found in [22], and only a few changes are necessary to adapt it to other robots; in [23], the same authors presented the Denavit–Hartenberg parameters for the Open Unit Robot (OUR) manipulator created by Smokie Robotics.

To the best of the authors’ knowledge, a complete IK solution that integrates the calculation of singularities and a strategy to circumvent them has not been published. Therefore, this work provides a practical IK solution that manages singularities besides the multiplicity of angle solutions inherent to the IK problem, and constitutes a ready-to-use tool for developing, simulating, and controlling robotic systems.

Additionally, two modalities or methods for dealing with singularities and multiple solution sets are proposed. A finite state machine (FSM) is used to tailor one of the methods for complementing the IK analysis with the direct production of an appropriate nonsingular solution without calculating all the possible solutions. An IK solution that avoids singular configurations is helpful in designing and controlling a robot and its digital twin [24], and in image-based visual servoing [25].

This paper presents a complete IK solution, a compact expression for the determinant of the Jacobian that can be easily implemented, and two effective methods with their corresponding algorithms for handling singularities and choosing a set of angles or calculating a single one. The rest of this paper is organized as follows: Section 2 shows a set of expressions used to compute the IK solution, and presents the singularity analysis for robots with the UR geometry and two algorithms that can be used to choose a set of angles. The results obtained with the two algorithms are compared in Section 3. Lastly, conclusions are presented in Section 4.

2. Materials and Methods

First, this section shows the IK solution on which the selection algorithms are based; then, it describes the singularity problem and presents the singularity analysis for robots with the same geometry as the UR5; lastly, the proposed selection algorithms are described.

2.1. Inverse Kinematic Solution

The UR5, which is the specific robot that was used for the experiments, is a 6-degree-of-freedom (DOF) cobot that only has rotational joints.

Table 1. Denavit–Hartenberg (DH) parameters of the UR5.

i	${\mathit{\alpha }}_{\mathit{i}}\left(\mathbf{rad}\right)$	${\mathit{a}}_{\mathit{i}}\left(\mathbf{m}\mathbf{m}\right)$1	${\mathit{d}}_{\mathit{i}}\left(\mathbf{mm}\right)$1	${\mathit{\theta }}_{\mathit{i}}$
1	$\pi /2$	0	$d_1=89.2$	${\theta }_1$
2	0	$a_2=425.0$	0	${\theta }_2$
3	0	$a_3=392.0$	0	${\theta }_3$
4	$\pi /2$	0	$d_4=109.3$	${\theta }_4$
5	$-\pi /2$	0	$d_5=94.75$	${\theta }_5$
6	−	−	$d_6=82.5$	${\theta }_6$

¹ The values for these parameters were obtained from [26].

presents its DH parameters, which are illustrated in Figure 1. However, the IK solution and the singularity analysis only use variables, because this allows for using them for any other robot with the same geometry.

From the different IK solutions found in the literature, only one was considered to define the algorithms presented here; however, these algorithms can be easily modified to use a solution that calculates the angles in a different order. The following notation is used in this work: $s_i=\sin {\theta }_i$, $c_i=\cos {\theta }_i$, $s_{ij\dots }=\sin \left({\theta }_i+{\theta }_j+\dots \right)$ and $c_{ij\dots }=\cos \left({\theta }_i+{\theta }_j+\dots \right)$. The homogeneous transformation matrix from Equation (1) is composed of a rotational submatrix (elements represented by r) and a position vector (p elements); these define the orientation and position of frame 6 with respect to the base (frame 0). Since the DK solution is required to compute the IK solution, the resulting transformation matrix and its elements are shown next [10]:

$${}_{\mathbf{6}}{}^{\mathbf{0}}\mathit{T}=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]$$

(1)

$$r_{11}=c_1{c}_{234}{c}_5{c}_6+c_6{s}_1{s}_5-c_1{s}_{234}{s}_6$$

(2)

$$r_{21}=c_{234}{c}_5{c}_6{s}_1-c_1{c}_6{s}_5-s_1{s}_{234}{s}_6$$

(3)

$$r_{31}=c_5{c}_6{s}_{234}+c_{234}{s}_6$$

(4)

$$r_{12}=-c_1{c}_{234}{c}_5{s}_6-s_1{s}_5{s}_6-c_1{c}_6{s}_{234}$$

(5)

$$r_{22}=-c_{234}{c}_5{s}_1{s}_6+c_1{s}_5{s}_6-c_6{s}_1{s}_{234}$$

(6)

$$r_{32}=-c_5{s}_{234}{s}_6+c_{234}{c}_6$$

(7)

$$r_{13}=-c_1{c}_{234}{s}_5+c_5{s}_1$$

(8)

$$r_{23}=-c_{234}{s}_1{s}_5-c_1{c}_5$$

(9)

$$r_{33}=-s_{234}{s}_5$$

(10)

$$p_x=r_{13}{d}_6+c_1\left(s_{234}{d}_5+c_{23}{a}_3+c_2{a}_2\right)+s_1{d}_4$$

(11)

$$p_y=r_{23}{d}_6+s_1\left(s_{234}{d}_5+c_{23}{a}_3+c_2{a}_2\right)-c_1{d}_4$$

(12)

$$p_z=r_{33}{d}_6-c_{234}{d}_5+s_{23}{a}_3+s_2{a}_2+d_1$$

(13)

Although some equations are expressed differently, the IK solution used in this work consists of the following expressions that were proposed in [8]:

$$A=p_y-d_6{r}_{23}$$

(14)

$$B=p_x-d_6{r}_{13}$$

(15)

$$C=c_1{r}_{11}+s_1{r}_{21}$$

(16)

$$D=c_1{r}_{22}-s_1{r}_{12}$$

(17)

$$E=s_1{r}_{11}-c_1{r}_{21}$$

(18)

$$F=c_5{c}_6$$

(19)

$${\theta }_1=\pm \mathrm{atan}2\left(\sqrt{B^2+{\left(-A\right)}^2-d_4^2},d_4\right)+\mathrm{atan}2\left(B,-A\right)$$

(20)

$${\theta }_5=\pm \mathrm{atan}2\left(\sqrt{E^2+D^2},s_1{r}_{13}-c_1{r}_{23}\right)$$

(21)

$${\theta }_6=\mathrm{atan}2\left(\frac{D}{s_5},\frac{E}{s_5}\right)$$

(22)

$${\theta }_{234}=\mathrm{atan}2(r_{31}F-s_{6}C,FC+s_6{r}_{31})$$

(23)

$$K_C=c_1{p}_x+s_1{p}_y-s_{234}{d}_5+c_{234}{s}_5{d}_6$$

(24)

$$K_S=p_z-d_1+c_{234}{d}_5+s_{234}{s}_5{d}_6$$

(25)

$$c_3=\frac{K_S^2+K_C^2-a_2^2-a_3^2}{2a_2{a}_3}$$

(26)

$$s_3=\sqrt{1-c_3^2}$$

(27)

$${\theta }_3=\pm \mathrm{atan}2\left(s_3,c_3\right)$$

(28)

$${\theta }_2=\mathrm{atan}2\left(K_S,K_C\right)-\mathrm{atan}2\left(s_3{a}_3,c_3{a}_3+a_2\right)$$

(29)

$${\theta }_4={\theta }_{234}-{\theta }_2-{\theta }_3$$

(30)

2.2. Singularities

The Jacobian is a matrix that relates the joint velocities to the linear and angular velocities of the end effector. This matrix is used to find singularities because, for nonredundant robots such as the UR5, they exist when $\det \left(\mathit{J}\right)=0$; the Jacobian can be computed using different methods, one of which is shown in [27].

In [28], the authors mentioned that a robot is at a singularity or singular configuration when it is impossible to move the end effector in at least one direction; there, the Jacobian was computed as in Equations (31) and (32) for prismatic and rotational joints, respectively.

$$\mathit{J}=\left[\begin{array}{c}{\mathit{J}}_{L_i}\\ {\mathit{J}}_{A_i}\end{array}\right]=\left[\begin{array}{c}{\mathit{b}}_{i-1}\\ \mathbf{0}\end{array}\right]$$

(31)

$$\mathit{J}=\left[\begin{array}{c}{\mathit{J}}_{L_i}\\ {\mathit{J}}_{A_i}\end{array}\right]=\left[\begin{array}{c}{\mathit{b}}_{i-1}\times {\mathit{r}}_{i-1,e}\\ {\mathit{b}}_{i-1}\end{array}\right]$$

(32)

In the previous equations, ${\mathit{b}}_{i-1}$ is the unit vector representing the z-axis of joint $i-1$ with respect to the base (frame 0), ${\mathit{r}}_{i-1,e}$ is the end-effector position with respect to frame $i-1$, and ${\mathit{J}}_{L_i}$ and ${\mathit{J}}_{A_i}$ represent the parts of the Jacobian that relate the joint velocities to the linear and angular ones, respectively. An example of how the Jacobian matrix is computed using this method can be found in [28].

Vectors ${\mathit{b}}_{i-1}$ and ${\mathit{r}}_{i-1,e}$ can be obtained from the DK study because ${\mathit{b}}_{i-1}$ is equal to ${}^{\mathbf{0}}{\widehat{\mathit{z}}}_{\mathit{i}-\mathbf{1}}$ (third column of rotational matrix ${}_{\mathit{i}-\mathbf{1}}{}^{\mathbf{0}}\mathit{R}$), and ${\mathit{r}}_{i-1,e}$ can be obtained by subtracting the translation vector ${}_{\mathit{i}-\mathbf{1}}{}^{\mathbf{0}}\mathit{P}$ from the end-effector position.

Since joint velocities tend to infinity as the robot approaches a singular configuration, studying and avoiding singularities is necessary to help in rendering the interaction between robot and user safer.

2.3. Singularity Analysis

Only Equation (32) is necessary to compute the Jacobian because the UR5 consists exclusively of rotational joints. Adapting the Jacobian matrix with the corresponding UR5 parameters, the following can be expressed:

$${\mathit{J}}_A=\left[\begin{array}{cccccc}0& s_1& s_1& s_1& c_1{s}_{234}& r_{13}\\ 0& -c_1& -c_1& -c_1& s_1{s}_{234}& r_{23}\\ 1& 0& 0& 0& -c_{234}& r_{33}\end{array}\right]$$

(33)

$${\mathit{r}}_{0,e}=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$$

(34)

$${\mathit{r}}_{1,e}=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z-d_1\end{array}\right]$$

(35)

$${\mathit{r}}_{2,e}=\left[\begin{array}{c}{p}_x-c_1{c}_2{a}_2\\ p_y-c_2{s}_1{a}_2\\ p_z-s_2{a}_2-d_1\end{array}\right]$$

(36)

$${\mathit{r}}_{3,e}=\left[\begin{array}{c}{p}_x-c_1{c}_{23}{a}_3-c_1{c}_2{a}_2\\ p_y-c_{23}{s}_1{a}_3-c_2{s}_1{a}_2\\ p_z-s_{23}{a}_3-s_2{a}_2-d_1\end{array}\right]$$

(37)

$${\mathit{r}}_{4,e}=\left[\begin{array}{c}{r}_{13}{d}_6+c_1{s}_{234}{d}_5\\ r_{23}{d}_6+s_1{s}_{234}{d}_5\\ r_{33}{d}_6-c_{234}{d}_5\end{array}\right]$$

(38)

$${\mathit{r}}_{5,e}=\left[\begin{array}{c}{r}_{13}{d}_6\\ r_{23}{d}_6\\ r_{33}{d}_6\end{array}\right]$$

(39)

The previous results allow computing ${\mathit{J}}_L$ as shown next:

$${\mathit{J}}_{L_1}=\left[\begin{array}{c}-p_y\\ p_x\\ 0\end{array}\right]$$

(40)

$${\mathit{J}}_{L_2}=\left[\begin{array}{c}-c_1\left(p_z-d_1\right)\\ -s_1\left(p_z-d_1\right)\\ s_1{p}_y+c_1{p}_x\end{array}\right]$$

(41)

$${\mathit{J}}_{L_3}=\left[\begin{array}{c}{c}_1\left(s_{234}{s}_5{d}_6+c_{234}{d}_5-s_{23}{a}_3\right)\\ s_1\left(s_{234}{s}_5{d}_6+c_{234}{d}_5-s_{23}{a}_3\right)\\ -c_{234}{s}_5{d}_6+s_{234}{d}_5+c_{23}{a}_3\end{array}\right]$$

(42)

$${\mathit{J}}_{L_4}=\left[\begin{array}{c}{c}_1\left(s_{234}{s}_5{d}_6+c_{234}{d}_5\right)\\ s_1\left(s_{234}{s}_5{d}_6+c_{234}{d}_5\right)\\ -c_{234}{s}_5{d}_6+s_{234}{d}_5\end{array}\right]$$

(43)

$${\mathit{J}}_{L_5}=\left[\begin{array}{c}-d_6\left(s_1{s}_5+c_1{c}_{234}{c}_5\right)\\ d_6\left(c_1{s}_5-c_{234}{c}_5{s}_1\right)\\ -c_5{s}_{234}{d}_6\end{array}\right]$$

(44)

$${\mathit{J}}_{L_6}=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$$

(45)

As previously mentioned, to verify if the combination of joint angles results in a singularity, the determinant of the Jacobian matrix must be computed to determine if it is equal to or close to zero. This can be conducted with the following expression:

$$\det \left(\mathit{J}\right)=s_3{s}_5{a}_2{a}_3\left(c_2{a}_2+c_{23}{a}_3+s_{234}{d}_5\right)$$

(46)

Three types of singularities (shoulder, wrist, and elbow) exist for this robot. Information about them can be found in [20,21], and they are briefly described next.

A shoulder singularity happens when the last factor in Equation (46), which involves angles ${\theta }_2$, ${\theta }_3$, and ${\theta }_4$, is equal to zero. One example can be seen in Figure 2, which shows that the end effector cannot be moved along $z_6$.

Wrist singularities exist when $s_5=0$, which mathematically happens when ${\theta }_5=0$ or ${\theta }_5=\pm \pi$. This renders $z_4$ and $z_6$ parallel.

An elbow singularity is present when $s_3=0$, which happens when ${\theta }_3=0$ or ${\theta }_3=\pm \pi$. This means that the arm is fully stretched or bent; however, only the former case is physically possible.

2.4. Algorithms to Select a Solution

This section presents two complete algorithms that can be used to select one set of angles that does not result in a singularity. In this work, angles close to 0 were considered to be 0, and angles close to $\pm \pi$ were defined as $\pi$; $2\pi$ was added or subtracted until the computed angle was within $[-\pi ,\pi ]$.

Not all values computed by the IK solutions are valid, i.e., some of them lead to computational errors. The cases in which this happens are described next:

• Computed ${\theta }_1$ angles are not acceptable if they are complex and no valid set can be computed. For the used IK solution, it is enough to validate the result of the square root in Equation (20).
• ${\theta }_5$ is not acceptable if it is complex or if $|s_5|\le 1\times {10}^{-12}$; since the IK solution uses atan2, only the latter validation is necessary. The value of the limit for $|s_5|$ was chosen because if the other sines and cosines in Equation (46) are equal to 1, it results in $\det \left(\mathit{J}\right)=1.5190\times {10}^{-4}$, which can be considered equal to zero. The other reason was that although $\sin \left(0\right)=\sin \left(\pi \right)=\sin \left(-\pi \right)=0$, the computational tools do not always give the exact value, e.g., in MATLAB R2021a $\sin \left(\pi \right)=1.2246\times {10}^{-16}$; this means that the determinant is not always exactly 0.
• The angles for ${\theta }_3$ are not valid if $|s_3|\le 1\times {10}^{-12}$, or when either ${\theta }_3$ or $s_3$ is complex. For the used IK solution, it is enough to validate $s_3$. The value defined for ${\theta }_3$ to be valid was obtained using the same considerations described previously for ${\theta }_5$.
• Angles ${\theta }_2$ and ${\theta }_4$ are not considered to be acceptable if $|d_5{s}_{234}+a_2{c}_2+a_3{c}_{23}|\le 1\times {10}^{-9}$; the value of the limit was chosen because if $s_3$ and $s_5$ are both equal to 1 and this term is $1\times {10}^{-9}$, then $\det \left(\mathit{J}\right)=1.6660\times {10}^{-4}$, which can be considered to be zero.
• Lastly, a complete set of angles is not considered to be valid when the manipulator reaches the outer limit of its workspace, which happens when ${\theta }_3=0$ and ${\theta }_4=\pi /2$. However, this leads to $s_3=0$, which was already defined as not valid.

Modifications to the following algorithms may be necessary depending on the application and the order in which the IK solution that is used computes the angles. These algorithms do not consider the presence of obstacles.

2.4.1. Algorithm 1

The first algorithm computes all the sets of angles that take the end effector to a previously specified pose, and then selects the one that requires moving the joints the least overall.

To select a set of variables, some works maximize a cost function depending on the objective (avoiding singularities, joint limits, or obstacles) [29] or find trajectories that do not include the mutation of any joint angles at ${180}^{\circ }$ [30]; however, this algorithm selects the set as in [31], where the total joint displacement is minimized. Although something similar was performed in [32], where the solution that minimized joint movement was selected, weights were used to prioritize moving smaller joints, which were assigned smaller weights.

Even though a specific selection criterion is used, depending on the objective, it is possible to change it without affecting the rest of the steps.

This algorithm consists of the following steps:

(1)

Both ${\theta }_1$ solutions are computed, and complex angles are discarded.

(2)

The previously obtained values for ${\theta }_1$ are used to compute ${\theta }_5$. The sets containing values of ${\theta }_5$ that are not considered valid are rejected.

(3)

${\theta }_6$ is computed for the remaining sets.

(4)

The values of ${\theta }_3$ are computed and verified. Again, the solutions with angles that are not acceptable are discarded.

(5)

Lastly, ${\theta }_2$ and ${\theta }_4$ are computed, and the sets of angles that are not valid are rejected.

(6)

The algorithm selects the solution with the minimal difference with respect to the current joint positions, which is computed with the following equations:

$$\Delta {\theta }_i={\theta }_{i,p}-{\theta }_{i,j}$$

(47)

and

$$dif{f}_j=\sqrt{\sum _{i=1}^6\mathsf{\Delta }{\theta }_i^2},$$

(48)

where p refers to a previous value, i to a joint, and j to the computed set.

2.4.2. Algorithm 2

When there are two possible solutions for an angle, the second algorithm chooses the one that moves the specific joint less and verifies it does not result in a singularity. This is to compute only one complete set of angles that results in the desired pose.

This algorithm uses the FSM shown in Figure 3; this technique was used because it is a comprehensible computational model. The FSM technique defines the steps that have to be followed to complete a specified sequential task; a state is triggered by an event or completion signal activated in the previous state. In the proposed FSM, each step computes one or two angles, selects the closest option (if two possibilities exist), and verifies if the chosen angles are valid. If a value is not acceptable, it is modified, and a previous state is triggered. In this case, the task is to compute and select angles that do not result in singular configurations.

The states are described next:

• State 1: The values of ${\theta }_1$ are computed and verified, and the one closer to the previous ${\theta }_1$ is selected. If a valid ${\theta }_1$ is found, State 5 is next.
• State 5: As for ${\theta }_1$, the two possible angles for ${\theta }_5$ are computed and verified, and the one closer to the previous value is selected. If an acceptable ${\theta }_5$ is found, State 6 is next. Since the difference between the values of ${\theta }_5$ is the sign, only one of them is necessary for the validation. If the computed angles are not valid, State 5 is repeated using the other ${\theta }_1$; however, no solution exists if ${\theta }_1$ has already been changed.
• State 6: Here, ${\theta }_6$ is computed. This is followed by State 3.
• State 3: As for ${\theta }_1$ and ${\theta }_5$, both ${\theta }_3$ values are computed and verified, and the one closer to the previous ${\theta }_3$ is selected; State 24 is run if an acceptable ${\theta }_3$ exists. Again, it is only necessary to verify one of the calculated angles; if ${\theta }_3$ is not valid, one of the following states is next:

  -

  State 6: if ${\theta }_5$ has not been modified, the other value for ${\theta }_5$ is used.

  -

  State 5: if ${\theta }_5$ has been changed and ${\theta }_1$ has not, the other possible angle for ${\theta }_1$ is tested.

  -

  End: no set of angles exists if, after changing ${\theta }_1$, both possibilities for ${\theta }_5$ result in unacceptable values for ${\theta }_3$.

• State 24: ${\theta }_2$ and ${\theta }_4$ are computed in this state; for this reason, it is called State 24. If the resulting set of angles is valid, the algorithm has found a solution (“End” is the next state); otherwise, one of the following states is run:

  -

  State 24: if the value of ${\theta }_3$ has not been changed, it is modified, and the state is repeated.

  -

  State 6: if both ${\theta }_3$ angles have been used and only one ${\theta }_5$ has been used, the other possible ${\theta }_5$ is tested.

  -

  State 5: if ${\theta }_5$ has been changed and both ${\theta }_3$ options have been tested, but ${\theta }_1$ has not been modified. The other angle ${\theta }_1$ is used.

  -

  End: if the manipulator cannot be taken to the desired pose even after changing ${\theta }_1$, ${\theta }_5$, and ${\theta }_3$.

• End: this last state is reached when a valid set of angles is found or if it is impossible to find one.

3. Results and Discussion

This section compares the results obtained with the algorithms presented in Section 2.4.

For the comparison, the desired poses are shown in

Table 2. Joint angles used to compute the desired poses $\left(\mathrm{rad}\right)$.

Test	${\mathit{\theta }}_1$	${\mathit{\theta }}_2$	${\mathit{\theta }}_3$	${\mathit{\theta }}_4$	${\mathit{\theta }}_5$	${\mathit{\theta }}_6$
1	$\pi$	$\pi /4$	$\pi /2$	$\pi /2$	0	$\pi /5$
2	$\pi$	$-\pi /2$	0	$\pi$	$\pi /3$	0
3	$\pi$	0	$\pi /2$	$\pi$	$\pi$	0
4	$\pi /2$	$\pi /2$	0	$-\pi /2$	$\pi /3$	$\pi$
5	$\pi /2$	0	$\pi /4$	$\pi$	$-\pi$	0
6	$-\pi /3$	$3\pi /4$	0	$\pi$	$-\pi$	0
7	$\pi /3$	$\pi /3$	$\pi /2$	$\pi /4$	$\pi /3$	0
8	$\pi /5$	$\pi /2$	$\pi /2$	$\pi /2$	$\pi$	$\pi /2$
9	$-\pi$	$\pi /3$	$-\pi /2$	$\pi /2$	$\pi /6$	$\pi /2$
10	$-\pi /2$	$\pi /2$	0	$\pi$	$-\pi$	$\pi /2$

(only sets 7 and 9 do not result in singularities). However, to choose a set of angles for said poses, the previous ones were assumed to be ${\theta }_1={\theta }_2={\theta }_3={\theta }_4={\theta }_5={\theta }_6=0$.

Both algorithms were programmed in MATLAB R2021a. Other results can be obtained depending on how the algorithms and IK solutions are programmed (e.g., if values close to $-\pi$ are defined as $-\pi$ and not as $\pi$, or if the joint angles are not limited to be within $[-\pi ,\pi ]$).

Table 3. Joint angles selected by Algorithm 1 $\left(\mathrm{rad}\right)$.

Test	${\mathit{\theta }}_1$	${\mathit{\theta }}_2$	${\mathit{\theta }}_3$	${\mathit{\theta }}_4$	${\mathit{\theta }}_5$	${\mathit{\theta }}_6$
1	$2.3815$	$0.7054$	$1.6608$	$0.7755$	$0.7601$	$1.4137$
2	$-1.7132$	$-2.0438$	$0.5154$	$2.5800$	$1.4998$	$1.4473$
3	$-0.6392$	$1.4741$	$1.9941$	$-0.3266$	$0.6392$	$-1.5708$
4	$1.5708$	$0.9088$	$1.3914$	$0.8414$	$-1.0472$	0
5	$-1.9116$	$2.2113$	$1.1655$	$-0.2352$	$0.3408$	$-0.7854$
6	$2.4303$	$0.2367$	$0.7022$	$2.2027$	$-0.3359$	$-2.3562$
7	$1.0472$	$1.0472$	$1.5708$	$0.7854$	$1.0472$	0
8	$-2.0715$	$-0.1327$	$1.5357$	$1.7386$	$-0.4418$	0
9	$3.1416$	$-0.4429$	$1.5708$	$-0.0807$	$0.5236$	$1.5708$
10	$-2.9992$	$0.9905$	$0.9407$	$1.2104$	$-1.7132$	0

and

Table 4. Joint angles selected by Algorithm 2 $\left(\mathrm{rad}\right)$.

Test	${\mathit{\theta }}_1$	${\mathit{\theta }}_2$	${\mathit{\theta }}_3$	${\mathit{\theta }}_4$	${\mathit{\theta }}_5$	${\mathit{\theta }}_6$
1	$2.3815$	$0.7054$	$1.6608$	$0.7755$	$0.7601$	$1.4137$
2	$-1.7132$	$-2.0438$	$0.5154$	$2.5800$	$1.4998$	$1.4473$
3	$-0.6392$	$1.4741$	$1.9941$	$-0.3266$	$0.6392$	$-1.5708$
4	$1.5708$	$0.9088$	$1.3914$	$0.8414$	$-1.0472$	0
5	$-1.9116$	$2.2113$	$1.1655$	$-0.2352$	$0.3408$	$-0.7854$
6	$2.4303$	$0.2367$	$0.7022$	$2.2027$	$-0.3359$	$-2.3562$
7	$-0.8445$	$0.5569$	$1.3797$	$2.0214$	$2.8289$	$0.9248$
8	$-2.0715$	$0.3281$	$1.0257$	$-1.3537$	$0.4418$	$3.1416$
9	$-0.3414$	$2.5392$	$0.8718$	$-1.9651$	$2.6900$	$-2.2975$
10	$-2.9992$	$0.9905$	$0.9407$	$1.2104$	$-1.7132$	0

present the sets of angles selected by Algorithms 1 and 2, respectively. Different results were chosen only in three tests (7, 8, and 9).

Table 5. Determinants computed with the selected joint angles $\left(\mathrm{rad}\right)$.

Test	Algorithm 1		Algorithm 2
1		$4.9919\times {10}^6$
2		$-7.7601\times {10}^6$
3		$-2.9926\times {10}^7$
4		$-0.1915$
5		$-3.2505\times {10}^7$
6		$-2.2865\times {10}^7$
7	$-2.1859\times {10}^7$		$0.7623\times {10}^7$
8	$-3.4649\times {10}^7$		$2.9645\times {10}^7$
9	$5.2815\times {10}^7$		$-3.5288\times {10}^7$
10		$-1.2625\times {10}^7$

shows the computed determinants and proves that the final sets did not result in singular configurations.

Table 6. Differences computed with the selected joint angles $\left(\mathrm{rad}\right)$.

Test	Algorithm 1		Algorithm 2
1		$3.4792$
2		$4.2870$
3		$3.0888$
4		$2.6521$
5		$3.2697$
6		$4.1197$
7	$2.5247$		$3.9838$
8	$3.1441$		$4.1651$
9	$3.9091$		$4.8683$
10		$3.9066$

presents the differences between the selected angles and the previous ones; these were computed as in Algorithm 1 using Equation (48) and prove that Algorithm 2 sometimes chose a set that required moving the joints more than Algorithm 1.

3.1. Discussion

The results show that the algorithms did not always choose the same angles. However, selecting any of the computed sets that did not result in a singularity render it safer for the user to interact with the robot.

Although Algorithm 1 selected the angles that moved the robot the least, which means that the robot would reach the desired pose faster, Algorithm 2 would not lead to abrupt movements, which is even safer for the user.

It is recommended to use Algorithm 1 when it is necessary to choose the set of angles that moves the joints the least; however, the memory of the device used to compute the IK solution should be enough to store up to eight possibilities. Algorithm 2 is suggested for applications using devices with low computational resources and when the new configuration does not need to be as similar as possible to the previous one.

The derived expression and proposed methods to compute the determinant of the Jacobian require fewer parameters and calculations than the ones found in other works; this implies a computational advantage particularly useful for real-time applications. The proposed methods need modifications to use the solution in [20]. Even though it is possible to use the equation in [21] in the proposed algorithms, the determinant does not result in zero when either ${\theta }_3$ or ${\theta }_5$ is equal to $\pi$; these mathematical singularities exist and can be detected with Equation (46).

3.2. Applications

A complete IK solution that avoids singularities is helpful in different applications; one of them is robot design, particularly to implement the inverse kinematic control of a robot with the same geometry. Other more advanced applications include image-based visual servoing and the control of robotic digital twins.

Image-based visual servoing, as seen in Figure 4, refers to the use of features extracted from images to move the robot to a desired feature or pose. In the latter case, it is necessary to include the IK solution as a part of an external controller to choose one set of angles that can move the end effector as desired while avoiding singularities.

Using a digital twin as supervisory control of a physical robot requires the IK solution to be implemented in the digital twin controller to verify that the physical system is working as expected and under safe conditions, which is particularly important for cobots. The proposed validated IK solution that uses the FSM is suitable because it avoids abrupt changes in the joint motion, this also renders the system safer because joint velocities can be safely controlled if singularities can be detected and avoided.

4. Conclusions

Inverse kinematic solutions can be used to control real and virtual robots. However, when an IK solution is used, it is also necessary to consider the robot’s singular configurations to have complete and validated robot control algorithms. For that reason, this study used the Jacobian matrix of the UR5 robot and its determinant; the latter was used in two algorithms to select a set of angles that takes the robot to the desired pose without resulting in a singularity. In this way, the main contribution of this work is the definition of two alternative methods for the complete and validated computational implementation of IK solutions for the UR5 robot. Since reaching singularities leads to faster joint velocities, methods that help in avoiding singular configurations and in making it safer for people to interact with this kind of robot are essential additions to the IK solution and can be considered to be enhanced alternative inverse IK solutions for robotic arms with the UR geometry.

The results show that, although the two algorithms could lead to the same set of angles, this is not always true, which means that one of the presented algorithms can be more suitable depending on the application. For example, if one objective is to reach the desired pose faster, Algorithm 1 can be used to ensure that the set of angles that requires moving the joints the least overall is selected; however, by using an FSM and not computing all the possibilities, Algorithm 2 requires less storage space, which means that it can be used in devices with low computational resources; this algorithm also avoids abrupt joint movements.

As a final remark, the FSM-based method complements the IK solution procedure with advantages in the number of computations and performance by producing results that would not move the joints abruptly, which is desired for collaborative robots, and this method is helpful when using devices with low computational resources.

Future work will focus on using the IK solution and one of the selection algorithms to control a virtual UR5 that will later be used to control a physical UR5 robot. The chosen algorithm will be modified to select the set of angles that consumes the least power or to evaluate which one results in the trajectory that moves the robot as far as possible from singularities.