2.1. Mapping from task space to joint positions set

To prevent the JDP and algorithmic singularity induced by the Jacobian-based algorithm, the IK problem is solved by two mapping processes instead of directly calculating the joint angles. For the first mapping $h\!\left ( \cdot \right ):\;{\mathbb{R}^{m}} \to{\textbf{S}}_{ P}$, the Cartesian position of the most important elbow joint (i.e., ${\textbf{P}}_{3}$) can be derived by spatial iteration with the consideration of link length and joint limits. As shown in Fig. 1, ${\textbf{P}}_{1}$ and ${\textbf{P}}_{5}$ can be excluded from the iteration due to the fixed base and desired $EE$ pose. Thus, the determined target (i.e., ${{\textbf{P}}}_{nt}$) is for ${\textbf{P}}_{4}$, and the iteration process is given by the following two phases:

(6) \begin{equation}{\textbf{S}}_{1p} \buildrel \Delta \over = \left \{ \begin{array}{l}{}^{i}{{\textbf{P}}^{\prime}}_{\!\!4} ={{{\textbf{P}}}_{nt}} ={{{{\textbf{P}}}_{des}} -{R_{ des}}{{\left [{{}_{n - 1}^{n}x,{}_{n-1}^ny,{}_{n-1}^{n}z} \right ]}^{T}}} \!\left ({n = 7} \right ) \\[5pt]{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3} ={}^{i}{{\textbf{P}}^{\prime}}_{\!\!4} + \widehat{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!4}{{}^{i-1}{{\textbf{P}}}_{3}}} \cdot{l_{3}},\, \theta _{6}^{\min } \le{\theta _{6}} \le \theta _{ 6}^{\max }\, \mathbf{or} \,{}^i{{\textbf{P}}^{\prime}}_{\!\!3} ={}^{i}{{\textbf{P}}^{\prime}}_{\!\!4} +{R_{{\textbf{u}}_1}}\!\left ( \Delta{\varphi _{6}} \right ) \widehat{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!4}{{}^{i-1}{{\textbf{P}}}_{3}}} \cdot{l_{3}} \\[5pt]{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!2}} ={{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}} + \widehat{{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!3}}{{}^{i-1}{{\textbf{P}}}_{2}}} \cdot{l_{2}},\, \theta _{4}^{\min } \le{\theta _{4}} \le \theta _{4}^{\max }\, \mathbf{or}\,{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!2}} ={{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}} +{R_{{\textbf{u}}_2}}\!\left ( \Delta{\varphi _{4}} \right ) \widehat{{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}}{{}^{i-1}{{\textbf{P}}}_{2}}} \cdot{l_{2}}, \end{array} \right. \end{equation}

in which ${{\textbf{u}}}_{1} = -\widehat{{{\textbf{l}}_{4}}} \times \widehat{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!4}{{}^{i-1}{{\textbf{P}}}_{3}}}$ and ${{\textbf{u}}}_{2} = \widehat{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!4}{{}^{i-1}{{\textbf{P}}}_{3}}} \times \widehat{{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!3}}{{}^{i-1}{{\textbf{P}}}_{2}}},$ and

(7) \begin{equation}{\textbf{S}}_{2p} \buildrel \Delta \over = \left \{ \begin{array}{l}{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}} ={\left [{0,0,\widehat{{{{\textbf{z}}}_{0}}} \cdot{l_{1}}} \right ]^{T}}\\[5pt]{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}} ={{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}} + \widehat{{{}^{ i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}}{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}}} \cdot{l_{2}},\, \theta _{2}^{\min } \le{\theta _{2}} \le \theta _{2}^{\max }\, \mathbf{or}\,{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3} ={{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}} +{R_{{\textbf{u}}_3}} \!\left ( \Delta{\varphi _{2}} \right ) \widehat{{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}}{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}}} \cdot{l_{2}} \\[5pt]{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!4}} ={{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}} + \widehat{{{}^{ i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}}{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!4}}} \cdot{l_{3}},\, \theta _{4}^{\min } \le{\theta _{4}} \le \theta _{4}^{\max }\, \mathbf{or}\,{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!4}} ={{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}} +{R_{{\textbf{u}}_4}} \!\left ( \Delta{\varphi _{4}} \right ) \widehat{{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}}{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!4}}} \cdot{l_{3}},\\[5pt] \end{array} \right. \end{equation}

Table I. Mathematical notations.

Figure 1. Schematic of the 7-DOF SRS manipulator, where ${{\textbf{P}}}_{1}$ and ${{\textbf{P}}}_{5}$ indicate the base and end-effector positions, respectively, and ${{\textbf{P}}}_{i}\, (i=2,3,4)$ indicate the shoulder, elbow, and wrist positions in order.

in which ${{{\textbf{u}}}}_{3} = \widehat{{{{{\textbf{z}}}}_{0}}} \times \widehat{{{}^{ i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}}{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}}}$ and ${{\textbf{u}}}_{4} = \widehat{{{}^{ i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}}{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}}} \times \widehat{{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}}{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!4}}}$. When ${\theta _{i}}\!\left ({i = 2,4,6} \right )$ exceed joint limits, $ \Delta{\varphi _{i}}$ can be given by

\begin{equation*} \Delta{\varphi _{i}} = \left \{{\begin{array}{*{20}{l}}{\theta _{i}^{\max } -{\theta _{i}}\quad if\quad \theta _{i}^{\max } \lt{\theta _{i}},} \\[5pt]{\theta _{i}^{\min } -{\theta _{i}}\quad if\quad{\theta _{i}} \lt \theta _{i}^{\min }.} \end{array}} \right. \end{equation*}

Table II. Abbreviations.

$R_{{{\textbf{u}}}} \!\left ( \theta \right )$ represents the rotation around the axis ${\textbf{u}}$ by an angle $\theta$, which is given as

\begin{equation*} R_{{{\textbf{u}}}} \!\left ( \theta \right ) = \!\left ({\cos \theta } \right )I + \!\left ({\sin \theta } \right ){\left [{{\textbf{u}}} \right ]_ \times } + \!\left ({1 - \cos \theta } \right )\!\left ({{{\textbf{u}}} \otimes{{\textbf{u}}}} \right ), \end{equation*}

where $\left [{{\textbf{u}}} \right ]_ \times$ is the cross-product matrix of ${\textbf{u}}$. ${}^{ i}{{\textbf{P}}^{\prime}}_{\!\!n}$ and ${}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!n}$ $\!\left ({2 \le n \le 4} \right )$ denote the position of ${{\textbf{P}}}_{n}$ in Fig. 1 after the first and second phases of the $i$-th iteration, respectively. The superscript ${}^{i-1}$ indicates the joint positions after the last iteration, or the initial joint positions when $i = 1$. $l_{n}$ is the length of the $n$-th link. ${{\textbf{P}}}_{des}$ and $R_{des}$ are the position vector and rotation matrix of $T_{des}$, respectively. $_{n - 1}^{n}x$, $_{n - 1}^{n}y$, and $_{n - 1}^{n}z$ are the $x$, $y$, and $z$ coordinates of the origin of frame $\Sigma _{n-1}$ in frame ${\Sigma _{n}} \!\left ({1 \le n \le 7} \right )$. $\widehat{{{\textbf{z}}}}_{n}$ is the $z$-axis unit vector of frame $\Sigma _{n}$, and $\widehat{{{\textbf{l}}_{4}}} = R_{des}{\left [{{}_{ n-1}^{ n}x,{}_{n-1}^{n}y,{}_{n-1}^{n}z} \right ]}^{T}\!\left ({n = 7} \right )$ is the desired direction vector of the end link.

Due to the determined direction vector of the end link and the fixed base, ${}^i{{\textbf{P}}^{\prime}}_{\!\!4}$ and ${}^i{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}$ should be corrected at the beginning of each phase. Equations (6)–(7) are cycled through until the distance between ${}^i{{\textbf{P}}^{\prime\prime}}_{\!\!4}$ and ${{\textbf{P}}}_{nt}$ meets the Cartesian error requirement and the angles between adjacent links are within joint limits. The $EE$ position is subsequently deduced by ${{{\textbf{P}}}_{5}} ={}^i{{\textbf{P}}^{\prime\prime}}_{\!\!4} + \widehat{{\textbf{l}}_{4}} \cdot l_{4}$. Set ${\textbf{S}}_{P} \buildrel \Delta \over = \left \{{{{{\textbf{P}}}_{3}},{{{\textbf{P}}}_{4}} \in{\textbf{S}}_{ 2p},{{{\textbf{P}}}_{ 1}},{{{\textbf{P}}}_{2}},{{{\textbf{P}}}_{5}}} \right \}$, containing all joint positions, is obtained after all iterations are completed. The advantages of the proposed algorithm are presented below, including theoretical analyses demonstrating its applicability to RMP.

$(1)$ JDF motion: When there are no external disturbances, each iteration phase and the final configuration are predetermined before the first phase with the given initial state vector ${\textbf{q}}_{0} \in{\mathbb{R}^{7}}$, ${{\textbf{P}}}_{nt}$, and the Cartesian error. The initial joint positions (i.e., ${}^{0}{{\textbf{P}}_{3}},{}^{0}{{\textbf{P}}_{4}}$) for the iteration of $h\!\left ( \cdot \right )$ can be obtained by forward kinematics with ${\textbf{q}}_{0}$. Once the above variables are determined, Eqs. (6)–(7) can guarantee the uniqueness of $h\!\left ( \cdot \right )$, that is, ${\textbf{S}}_{P}$ is unique for each IK query.

$(2)$ Continuity of links’ configurations $\left \{{\overrightarrow{{\textbf{l}}_{1}},\overrightarrow{{\textbf{l}}_{2}},\overrightarrow{{\textbf{l}}_{3}},\overrightarrow{{\textbf{l}}_{4}} } \right \}$: For motion planning tasks, the Cartesian path can be interpolated as consecutive path points. Meanwhile, the pose of the end link can be guaranteed because ${{{\textbf{P}}}_{ 5}}$ is not involved in iterations. In addition, since $\overrightarrow{{\textbf{l}}_{1}}$ is a constant vector, the key to iteration is to find the elbow position (i.e., ${{{\textbf{P}}}_{3}}$). Assuming ${{\textbf{P}}}_t$ and $\underline{{{\textbf{P}}}_{t}}={{\textbf{P}}}_{t} + \overrightarrow{\Delta{{\textbf{P}}}}$ are two consecutive path points that are treated as the targets for ${{\textbf{P}}}_{4}$, for the iteration of $\underline{{{\textbf{P}}}_{t}}$, the position change of ${{\textbf{P}}}_{3}$ in different phases can be summarized as

(8) \begin{equation} \left \{ \begin{array}{l} \underline{{}^{i}{P^{\prime}}_{\!\!3}} = \!\left ({{P_t} + \overrightarrow{\Delta p} } \right ) + \dfrac{{\underline{^{ i-1}{P_{3}}} - \!\left ({{P_{t}} + \overrightarrow{\Delta p} } \right )}}{{{{\left \|{\underline{^{i-1}{P_{3}}} - \!\left ({{P_{t}} + \overrightarrow{\Delta p} } \right )} \right \|}_{2}}}}{l_{3}}\\[20pt] \underline{{}^{i}{P^{\prime\prime}}_{\!\!\!3}} = \underline{{}^{i}{P^{\prime\prime}}_{\!\!\!2}} + \dfrac{{\underline{{}^{i}{P^{\prime}}_{\!\!3}} - \underline{{}^{ i}{P^{\prime\prime}}_{\!\!\!2}} }}{{{{\left \|{\underline{{}^{i}{P^{\prime}}_{\!\!3}} - \underline{{}^{i}{P^{\prime\prime}}_{\!\!\!2}} } \right \|}_{2}}}}{l_{2}}, \end{array} \right. \end{equation}

where the underline is used to indicate the iteration of $\underline{{{{\textbf{P}}}_{t}}}$. Using Eq. (8) to compare with the iteration process of ${{\textbf{P}}}_{t}$, the variation can be quantified as

(9) \begin{equation} \left \{ \begin{array}{l} \underline{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}} -{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3} = \overrightarrow{\Delta{{\textbf{P}}}} +{l_{3}} \Biggl ( \dfrac{{\underline{^{i - 1}{{{\textbf{P}}}_{3}}} - \!\left ({{{{\textbf{P}}}_{t}} + \overrightarrow{\Delta{{\textbf{P}}}} } \right )}}{{{{\left \|{\underline{^{i - 1}{{{\textbf{P}}}_{3}}} - \!\left ({{{{\textbf{P}}}_{t}} + \overrightarrow{\Delta{{\textbf{P}}}} } \right )} \right \|}_{2}}}} - \dfrac{{{}^{i - 1}{{{\textbf{P}}}_{3}} -{{{\textbf{P}}}_{t}}}}{{{\left \|{{}^{i - 1}{{{\textbf{P}}}_{3}} -{{{\textbf{P}}}_{t}}} \right \|}_{2}}} \Biggr ) \\[20pt] \underline{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}} -{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3} ={l_{2}} \Biggl ({\dfrac{{\underline{{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}} - \underline{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}} }}{{{{\left \|{\underline{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!3}} - \underline{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}} } \right \|}_{2}}}} - \dfrac{{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!3} -{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}}}{{{{\left \|{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!3} -{}^{ i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}} \right \|}_{2}}}}} \Biggr ), \end{array} \right. \end{equation}

where ${}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2} = \underline{{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!2}}$. Since $i$ starts from 1, the limit of Eq. (9) as $\overrightarrow{\Delta{{\textbf{P}}}}$ approaches $\mathbf{{0}}$ is given by

(10) \begin{equation} \left \{ \begin{array}{l} \mathop{\lim }\limits _{\overrightarrow{\Delta{{\textbf{P}}}} \to{\mathbf{{0}}}} \!\left ({\underline{{}^{ i}{{\textbf{P}}^{\prime}}_{\!\!3}} -{}^{i}{{\textbf{P}}^{\prime}}_{\!\!3}} \right ) ={\mathbf{{0}}} \\[11pt] \mathop{\lim }\limits _{\overrightarrow{\Delta{{\textbf{P}}}} \to{\mathbf{{0}}}} \!\left ({\underline{{}^{ i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}} -{}^{i}{{\textbf{P}}^{\prime\prime}}_{\!\!\!3}} \right ) ={\mathbf{{0}}}. \end{array} \right. \end{equation}

As shown in Eq. (10), the minor movement of ${{\textbf{P}}}_{4}$ in Cartesian space does not lead to the abrupt movement of ${{\textbf{P}}}_{3}$ with the same ${\textbf{q}}_{0}$. Equations (6)–(7) are the theoretical basis for generating smooth trajectories since they can be used to deduce continuous links’ configurations $\left \{{\overrightarrow{{\textbf{l}}_{1}},\overrightarrow{{\textbf{l}}_{2}},\overrightarrow{{\textbf{l}}_{3}},\overrightarrow{{\textbf{l}}_{4}} } \right \}$ for continuous path points.

$(3)$ Guaranteed $EE$ pose: ${{\textbf{P}}}_{5}$ is excluded from the iteration to improve efficiency and induce only position error with no orientation error of $EE$, which eliminates the effect of repeated rotation of the 7th joint on other links compared to the pseudo-inverse algorithms. When performing repetitive tip rolling motions, such as screwing tasks, the elbow joint does not swing due to the repetitive rotation of the 7th joint, which reduces the risk of collisions and improves the safety of operations.

$(4)$ Superior configurations: Generally, a higher $ w$ implies better singularity avoidance and adaptability during execution. In contrast with pseudo-inverse algorithms, Eqs. (6)–(7) can deduce the desired and natural configurations with higher $ w$. Furthermore, the solution process of the proposed algorithm is completed without using $J{\left ({\boldsymbol{\Theta }} \right )_{m \times n}}$, which compensates for the algorithmic singularity of the pseudo-inverse algorithms.

2.2. Mapping from joint positions set to the desired joint angle vector

An algorithmic solution implementing the mapping $g \!\left ( \cdot \right ) :\;{\textbf{S}}_{P} \to{{\boldsymbol{\Theta }} _{ des}}$ is given in this subsection, in which ${{\boldsymbol{\Theta }} _{des}} \in{\mathbb{R}^{7}}$ is the desired IK joint angle vector. Note that the solution uniqueness and continuity derived from $h \!\left ( \cdot \right )$ can be retained in the process of $g \!\left ( \cdot \right )$. $\left |{{\theta _{i}}} \right | \!\left ({i = 2,4,6} \right )$ are first calculated by

(11) \begin{equation} \left |{{\theta _i}} \right | = \left |{\pi - \arccos \!\left ({\frac{{l_{j}^{2} + l_{j + 1}^{2} -{{\!\left ({{{\left \|{{\textbf{P}}_{j + 2}} -{{\textbf{P}}}_{j} \right \|}_{2}}} \right )}^{2}}}}{{2{l_{j}}{l_{j + 1}}}}} \right )} \right |, \end{equation}

where $ i=2j$. $\left |{{\theta _{i}}} \right | \!\left ({i = 2,4,6} \right )$ will appear in the denominator of some equations and terminate calculations when one of them approaches or equals to zero. Hence, the realization of $g \!\left ( \cdot \right )$ can be classified into the following cases:

(1) $\theta _{2}$, $\theta _{4}$, and $\theta _{6}$ are not equal to zero: $\theta _{1}$ is first calculated by

(12) \begin{equation}{\!\left ({{A_{1}}{A_{2}}{A_{3}}{{\left [{0,0,0,1} \right ]}^{T}}} \right )_{1,1}} ={{{\textbf{P}}}_{3,x}}, \end{equation}

where the subscripts $i, j$ indicate the value of the $i$-th row and $j$-th column of the corresponding matrix, and $x$ indicates the $x$ coordinate of the corresponding joint Cartesian position. $A_{n}$ is the transformation matrix of frame $\Sigma _{n}$ with respect to frame $\Sigma _{n - 1}$. However, because the sign of $\theta _{2}$ cannot be determined temporally, the combinations of $\theta _{1}$ and $\theta _{2}$ should be filtered with the following equations

(13) \begin{equation} \theta _{i}^{\min } \le{\theta _{i}} \le \theta _{i}^{\max }, \end{equation}

(14) \begin{equation} \left |{1 - \widehat{{{\textbf{z}}}_{i}} \cdot \widehat{{{\textbf{l}}_{i/2 + 1}}}} \right | \le{\varepsilon _{ 1}}\, \big (i = 2,4,6 \big ), \end{equation}

where ${\varepsilon _{1}} ={10^{-3}}$ is the defined tolerance. The solution set ${\textbf{C}}_{1,2}$ is defined as

(15) \begin{equation}{\textbf{C}}_{1,2} = \biggl \{ \!\left ({\theta _{1}},{\theta _{2}} \right ) \left | \textrm{Eqs}.\;(11)-(14)\right. \biggr \}. \end{equation}

Given the set ${\textbf{C}}_{1,2}$, $\theta _{3}$ is calculated by

(16) \begin{equation}{\!\left ({{A_{3}}{A_{4}}{A_{5}}{{\left [{0,0,0,1} \right ]}^{T}}} \right )_{1,1}} ={\!\left ({A_{2}^{-1}A_{1}^{-1}{{\left [{{{{\textbf{P}}}_{4}},1} \right ]}^{T}}} \right )_{1,1}}. \end{equation}

Thus, the set ${\textbf{C}}_{3,4}$ is defined as

(17) \begin{equation}{\textbf{C}}_{3,4} = \biggl \{ \!\left ({\theta _{3}},{\theta _{4}} \right ) \left | \textrm{Eqs}.\;(11),\, (13),\, (14),\, \textrm{and}\, (16) \right .\biggr \}. \end{equation}

Similarly, $\theta _{5}$ is calculated by

(18) \begin{equation} \!\left ({A_{5}}{A_{6}}{A_{7}}{\left [{0,0,0,1} \right ]^{T}} \right )_{1,1} = \Big ( A_{4}^{-1}A_{3}^{-1}A_{ 2}^{ -1}A_{ 1}^{ -1}{T_{des}}{\left [{0,0,0,1} \right ]^{T}} \Big )_{1,1}, \end{equation}

which is used to define the set

(19) \begin{equation}{\textbf{C}}_{5,6} = \biggl \{ \!\left ({\theta _{5}},{\theta _{6}} \right ) \left | \textrm{Eqs}.\;(11),\, (13),\, (14),\, \textrm{and}\, (18)\right. \biggr \}. \end{equation}

Finally, $\left |{{\theta _{7}}} \right |$, which is the angle between $\widehat{{\textbf{x}}_{6}}$ and $\widehat{{\textbf{x}}_{7}}$, is given by

(20) \begin{equation} \left |{{\theta _{7}}} \right | = \left |{\arccos \!\left ({\widehat{{\textbf{x}}_{6}} \cdot \widehat{{\textbf{x}}_{7}}} \right )} \right |, \end{equation}

where $\widehat{{\textbf{x}}_{6}}$ and $\widehat{{\textbf{x}}_{7}}$ are the first three rows of the first columns of $\mathop \prod \limits _{n = 1}^{6}{A_{n}}$ and $T_{des}$, respectively. The set ${\textbf{C}}_{1-7}$, which includes all possible IK solutions, is expressed as

(21) \begin{equation}{\textbf{C}}_{1-7} = \biggl \{{\boldsymbol{\Theta }}\, \left | \textrm{Eqs}.\;(15),\, (17),\, (19),\, \textrm{and}\, (20)\right. \biggr \}. \end{equation}

$(2)$ $\left |{{\theta _{2}}} \right | \ne 0$, $\left |{{\theta _{4}}} \right | = 0$, and $\left |{{\theta _{6}}} \right | = 0$: ${\textbf{C}}_{1,2}$ is derived in the same way as in the first case. Due to $\left |{{\theta _{4}}} \right | = \left |{{\theta _{6}}} \right | = 0$, the analytical expressions for the remaining joint angles are difficult to obtain. Thus, the remaining solution processes can be formulated as the NLopt problem, which is given by

(22) \begin{equation} \begin{array}{l}{\textrm{min}}\, \Big [ \!\left ({\theta _{3}} -{\boldsymbol{\Theta }}_{init,3} \right )^{2} + \!\left ({\theta _{5}} -{\boldsymbol{\Theta }} _{init,5} \right )^{2} + \!\left ({\theta _{7}} -{\boldsymbol{\Theta }} _{init,7} \right )^{2} \Big ] \\[5pt]{\textrm{s.t.}}\, \left |{{\theta _{3}} +{\theta _{5}} +{\theta _{7}}} \right | = \left |{\arccos \!\left ({{{\!\left ({A_{ 2}^{ -1}A_{1}^{-1}{T_{des}}} \right )}_{1,1}}} \right )} \right |\, \textrm{and}\, \textrm{Eq}.\;(13), \end{array} \end{equation}

in which ${\boldsymbol{\Theta }} _{init,i}$ denotes the $i$-th value of the initial joint angle vector of the manipulator (i.e., ${\boldsymbol{\Theta }} _{init}$). The NLopt problem can be solved by the Sequential Least Squares Quadratic Programming algorithm. In this case, ${\textbf{C}}_{1-7}$ is defined as ${\textbf{C}}_{1-7} = \left \{{\boldsymbol{\Theta }} \left | \textrm{Eqs}.\;(15),\, (22),\, \textrm{and}\, \theta _{4} = \theta _{6} = 0 \right. \right \}$.

$(3)$ $\left |{{\theta _{2}}} \right | = 0$, $\left |{{\theta _{4}}} \right | \ne 0$, and $\left |{{\theta _{6}}} \right | = 0$: The solutions to $\left |{{\theta _{1}} +{\theta _{3}}} \right |$ and $\left |{{\theta _{5}} +{\theta _{7}}} \right |$ are formulated as

(23) \begin{equation} \begin{array}{l}{\textrm{min}}\, \Big [ \big ({\theta _{1}} -{\boldsymbol{\Theta }} _{init,1} \big )^{2} + \big ({\theta _{3}} -{\boldsymbol{\Theta }} _{init,3} \big )^{2} \Big ] \\[5pt]{\textrm{s.t.}}\, \left |{{\theta _{1}} +{\theta _{3}}} \right | = \left |{\arccos \!\left ({\frac{{{T_{ des,1,3}}}}{{s_{4}}}} \right )} \right |\, \textrm{and}\,{\textrm{Eq}}.\;(13), \end{array} \end{equation}

where $s_{i}$ is the abbreviation of $\sin{\theta _{i}}$, and

(24) \begin{equation} \begin{array}{l}{\textrm{min}}\, \Big [ \big ({\theta _{5}} -{\boldsymbol{\Theta }} _{init,5} \big )^{2} +{\big ({\theta _{7}} -{\boldsymbol{\Theta }} _{init,7} \big )^{2}} \Big ] \\[5pt]{\textrm{s.t.}}\, \left |{{\theta _{5}} +{\theta _{7}}} \right | = \left |{\arccos \!\left ({\frac{T_{ des,3,1}}{-s_{4}}} \right )} \right | \, \textrm{and}\,{\textrm{Eq}}.\;(13). \end{array} \end{equation}

Equation (14) is utilized to eliminate some wrong combinations of $\!\left ({{\theta _{1}} +{\theta _{3}}} \right )$ and ${\theta _{ 4}}$. ${\textbf{C}}_{1-7}$ is thus given by ${\textbf{C}}_{1-7} = \{{\boldsymbol{\Theta }}\, \left | \textrm{Eqs}.\;(11),\, (14),\, (23),\, (24)\right .,\, \textrm{and}\,{\theta _{4} = \theta _{6} = 0}\}$.

$(4)$ $\left |{{\theta _{2}}} \right | = 0$, $\left |{\theta _{4}} \right | = 0$, and $\left |{\theta _{6}} \right | \ne 0$: Using Eq. (11), $\left |{\theta _{7}} \right |$ can be calculated by

(25) \begin{equation} \left |{{\theta _{7}}} \right | = \left |{\arccos \!\left ({\frac{T_{des,3,1}}{-s_{6}}} \right )} \right |. \end{equation}

Then, ${\textbf{C}}_{6,7}$ is defined as

(26) \begin{equation}{\textbf{C}}_{6,7} = \biggl \{ \!\left ({\theta _{6}},\,{\theta _{7}} \right ) \left |{\textrm{Eqs}}.\;(11),\, (25),\,{\textrm{and}}\right. \left |{\widehat{{{\textbf{z}}}_{5}} \cdot \widehat{{\textbf{l}}_{4}}} \right | \le{\varepsilon _{ 1}} \biggr \}. \end{equation}

The solution to $\theta _{1}$, $\theta _{3}$, and $\theta _{5}$ is formulated as

(27) \begin{equation} \begin{array}{l}{\textrm{min}}\, \Big [ \big ({\theta _{1}} -{\boldsymbol{\Theta }} _{init,1} \big )^{2} +{ \big ({\theta _{3}} -{\boldsymbol{\Theta }} _{init,3} \big )^{2}} +{{\big ({{\theta _{5}} -{{\boldsymbol{\Theta }} _{init,5}}} \big )}^{2}} \Big ] \\[5pt]{\textrm{s.t.}}\, \left |{\theta _{1}} +{\theta _{3}} +{\theta _{5}} \right | = \left | \arccos \!\left ( \!\left ({T_{ des}A_{ 7}^{ - 1}A_{6}^{- 1}} \right )_{1,1} \right ) \right |\, \textrm{and}\, \textrm{Eq}.\;(13). \end{array} \end{equation}

Then, we can obtain ${\textbf{C}}_{1-7} = \left \{{\boldsymbol{\Theta }} \left | \textrm{Eqs}.\;(26),\, (27),\,{\textrm{and}}\right .{\theta _{2}} ={\theta _{4}} = 0 \right \}$.

$(5)$ $\left |{\theta _{2}} \right | \ne 0$, $\left |{{\theta _{4}}} \right | \ne 0$, and $\left |{{\theta _{6}}} \right | = 0$: The procedures in the first case are repeated until the sets ${\textbf{C}}_{1,2}$ and ${\textbf{C}}_{3,4}$ are obtained. Then, the solution to ${\theta _{5}}$ and ${\theta _{7}}$ can be written as

(28) \begin{equation} \begin{array}{l}{\textrm{min}}\, \Big [ \big ({\theta _{5}} -{\boldsymbol{\Theta }} _{init,5} \big )^{2} + \!\left ({\theta _{7}} -{\boldsymbol{\Theta }} _{init,7} \right )^{2} \Big ]\\[5pt]{\textrm{s.t.}}\, \left |{{\theta _{5}} +{\theta _{7}}} \right | = \left |{\arccos \!\left ({{{\!\left ({A_{ 4}^{ -1}A_{ 3}^{ -1}A_{2}^{-1}A_{1}^{-1}{T_{des}}} \right )}_{1,1}}} \right )} \right |\, \textrm{and}\,{\textrm{Eq}}.\;(13). \end{array} \end{equation}

Similarly, ${\textbf{C}}_{1-7}$ is thus defined as ${\textbf{C}}_{1-7} = \left \{{\boldsymbol{\Theta }}\, \left | \textrm{Eqs}.\;(15),\, (17),\, (28),\,{\textrm{and}}\right .{\theta _{6}} = 0 \right \}.$

$(6)$ $\left |{{\theta _{2}}} \right | = 0$, $\left |{{\theta _{4}}} \right | \ne 0$, and $\left |{{\theta _{6}}} \right | \ne 0$: Using Eq. (11) and $T_{des}$, $\left |{{\theta _{5}}} \right |$ should be calculated first by

(29) \begin{equation} \left |{{\theta _{5}}} \right | = \left |{\arccos \!\left ({\frac{{c_{4} c_{6} -{T_{des,3,3}}}}{{s_{4}s_{6}}}} \right )} \right |, \end{equation}

where $c_{i}$ is the abbreviation of $\cos{\theta _{i}}$. Given Eq. (29), ${\textbf{C}}_{5,6}$ is defined as

(30) \begin{equation}{\textbf{C}}_{5,6} = \biggl \{ \!\left ({\theta _{5}},{\theta _{6}} \right )\, \left | \textrm{Eqs}.\;(11),\, (13),\, \textrm{and}\, (29) \right .\biggr \}. \end{equation}

Then, $\theta _{7}$ is obtained by solving

(31) \begin{equation}{s_{4}}{s_{5}}{s_{7}} - \!\left ({{c_{4}}{s_{6}} +{c_{5}}{c_{6}}{s_{4}}} \right ){c_{7}} ={T_{des,3,1}} \end{equation}

with the constraints of Eqs. (14), (30), and $ \left |{1 + \widehat{{\textbf{y}}_{3}} \cdot \widehat{{\textbf{l}}_{2}}} \right | \le{\varepsilon _{1}}$. After deriving the set

(32) \begin{equation}{\textbf{C}}_{4,5,6,7} = \biggl \{ \!\left ({\theta _{4}},{\theta _{5}},{\theta _{6}},{\theta _{7}} \right )\, \left | \textrm{Eqs}.\;(11),\, (13),\, (30),\,{\textrm{and}}\, (31)\right. \biggr \}, \end{equation}

the solution to $\theta _{1}$ and $\theta _{3}$ is formulated as

(33) \begin{equation} \begin{array}{l}{\textrm{min}}\, \Big [ \big ({\theta _{1}} -{\boldsymbol{\Theta }} _{init,1} \big )^{2} +{\big ({\theta _{3}} -{\boldsymbol{\Theta }} _{init,3} \big )^{2}} \Big ] \\[5pt]{\textrm{s.t.}}\, \left |{{\theta _{1}} +{\theta _{3}}} \right | = \left |{\arccos \!\left ({{{\!\left ({{T_{ des}}A_{ 7}^{ -1}A_{ 6}^{-1}A_{5}^{-1}A_{4}^{-1}} \right )}_{1,1}}} \right )} \right |\, \textrm{and}\, \textrm{Eq}.\;(13). \end{array} \end{equation}

Finally, ${\textbf{C}}_{1-7}$ is defined as ${\textbf{C}}_{1-7} = \{ \Theta \, \left | \textrm{Eqs}.\;(32),\, (33),\, \textrm{and}\, \right .{\theta _{2}} = 0 \}$.

$(7)$ $\left |{{\theta _{2}}} \right | \ne 0$, $\left |{{\theta _{4}}} \right | = 0$, and $\left |{{\theta _{6}}} \right | \ne 0$: Given Eq. (11), $\theta _{7}$ can be calculated first with

(34) \begin{equation} \left |{{\theta _{7}}} \right | = \left |{\arccos \!\left ({\frac{{{{\!\left ({A_{2}^{-1}A_{1}^{-1}{T_{des}}} \right )}_{ 3,1}}}}{{-s_{ 6}}}} \right )} \right | \end{equation}

and ${\textbf{C}}_{1,2}$, which is similarly defined as in the first case. Then, ${\textbf{C}}_{6,7}$ is defined as

(35) \begin{equation}{\textbf{C}}_{6,7} = \biggl \{ \!\left ({\theta _{6}},{\theta _{7}}\right ) \left |{\textrm{Eqs}}.\;(11),\, (34),\,{\textrm{and}}\, \right. \left |{\widehat{{{\textbf{z}}}_{5}} \cdot \widehat{{\textbf{l}}_{4}}} \right | \le{\varepsilon _{1}} \biggr \}. \end{equation}

Thus, the solution to $\theta _{3}$ and $\theta _{5}$ can be formulated as

(36) \begin{equation} \begin{array}{l}{\textrm{min}}\, \Big [ \big ({\theta _{3}} -{\boldsymbol{\Theta }} _{init,3} \big )^{2} + \big ({\theta _{5}} -{\boldsymbol{\Theta }} _{init,5} \big )^{2} \Big ] \\[5pt]{\textrm{s.t.}}\, \left |{{\theta _{3}} +{\theta _{5}}} \right | = \left |{\arccos \!\left ({{{\!\left ({A_{2}^{ -1}A_{ 1}^{ -1}{T_{des}}A_{7}^{ - 1}A_{6}^{-1}} \right )}_{1,1}}} \right )} \right |\, \textrm{and}\, \textrm{Eq}.\;(13). \end{array} \end{equation}

${\textbf{C}}_{1-7}$ is then defined as ${\textbf{C}}_{1-7} = \{{\boldsymbol{\Theta }}\, \left | \textrm{Eqs}.\;(15),\, (35),\, (36),\, \textrm{and}\right .{\theta _{4}} = 0 \}$.

$(8)$ $\left |{{\theta _{2}}} \right | = 0$, $\left |{{\theta _{4}}} \right | = 0$, and $\left |{{\theta _{6}}} \right | = 0$: The solution to the remaining joint angles can be formulated as

(37) \begin{equation} \begin{array}{l}{\textrm{min}}\, \Big [ \big ({\theta _{1}} -{{\boldsymbol{\Theta }} _{init,1}} \big )^{2} + \big ({\theta _{3}} -{\boldsymbol{\Theta }} _{init,3} \big )^{2} +{\!\left ({\theta _{5}} -{\boldsymbol{\Theta }} _{init,5} \right )}^{2} + \!\left ({\theta _{7}} -{\boldsymbol{\Theta }} _{ init,7} \right )^{2} \Big ]\\[5pt]{\textrm{s.t.}}\, \big |{\theta _{1}} +{\theta _{3}} +{\theta _{5}} +{\theta _{7}} \big | = \big | \arccos \!\left ({{T_{ des,1,1}}} \right ) \big |\, \textrm{and}\, \textrm{Eq}.\;(13). \end{array} \end{equation}

Hence, ${\textbf{C}}_{1-7}$ is given as ${\textbf{C}}_{1-7} = \{{\boldsymbol{\Theta }}\, \left |{{\theta _{2}} ={\theta _{4}} ={\theta _{6}} = 0}\, \textrm{and}\, \textrm{Eq}.\;(37)\right. \}$.

No matter which case is performed, ${\textbf{C}}_{1-7}$ will eventually be defined to obtain ${\boldsymbol{\Theta }} _{ des}$. Traversing ${\textbf{C}}_{1-7}$ with

(38) \begin{equation} \begin{array}{l}{T_{temp}}\!\left ({\boldsymbol{\Theta }} \right ) ={A_{1}}{A_{2}} \cdots{A_{7}} = \left [{\begin{array}{*{20}{c}}{{R_{temp}}}\;\;\;\;&{{{{\textbf{P}}}_{temp}}}\\[5pt] 0\;\;\;\;&1 \end{array}} \right ], \end{array} \end{equation}

each ${T_{temp}}\!\left ({\boldsymbol{\Theta }} \right )$ can be used to compare with $T_{des}$ to derive the set ${\textbf{E}} \subseteq{\textbf{C}}_{1-7}$, which is defined as

(39) \begin{equation}{\textbf{E}} = \biggl \{{\boldsymbol{\Theta }} \;:\,\left ({{\varepsilon _{rot}} +{\varepsilon _{pos}}} \right ) \le{\varepsilon _{tol}},\,{\boldsymbol{\Theta }} \in{{\textbf{C}}_{1-7}} \biggr \}. \end{equation}

The rotation error $\!\left ( \textrm{i.e.,}\, \varepsilon _{rot}\right )$ and position error $\!\left ( \textrm{i.e.,}\, \varepsilon _{ pos}\right )$ can be calculated by

(40) \begin{equation} \begin{array}{l}{\varepsilon _{rot}} = \arccos \!\left ({\frac{{tr\left ({R_{temp}^{-1}{R_{des}}} \right ) - 1}}{2}} \right )\,{\textrm{and}} \,{\varepsilon _{pos}} = dist\!\left ({{{{\textbf{P}}}_{temp}},{{{\textbf{P}}}_{des}}} \right ) ={\left \|{{{{\textbf{P}}}_{ temp}} -{{{\textbf{P}}}_{des}}} \right \|_{2}}, \end{array} \end{equation}

respectively. Meanwhile, ${\boldsymbol{\Theta }} _{des}$ can be found by

(41) \begin{equation}{{\boldsymbol{\Theta }} _{des}} = \mathop{\arg \min }\limits _{{\boldsymbol{\Theta }} \in{\textbf{E}}}{\left \|{{\boldsymbol{\Theta }} -{{\boldsymbol{\Theta }} _{init}}} \right \|_{1}}. \end{equation}

Algorithm : Solving process of the proposed algorithm

Some joint angle elements in ${\boldsymbol{\Theta }}$ and ${\boldsymbol{\Theta }} _{init}$ can be omitted to reduce the variation in other joints and obtain other desired solutions. Although $\left |{{\theta _{i}}} \right | \!\left ({i = 2,4,6} \right )$ are rounded to a given number of places, ${\textbf{C}}_{1-7}$ in the first case has the most elements, and the multi-threaded solvers are required to boost the computational process to obtain ${\boldsymbol{\Theta }} _{des}$. For other cases, multithreading may reduce the efficiency of computation. The procedure of the proposed algorithm is outlined in Algorithm 1, where MultiThread is programmed to realize Eqs. (38)–(40) with multithreading, and MultiCase implements the solutions to Cases 2–8.

3.1. Joint-drift simulations

In this subsection, the joint-drift simulations are performed on the KUKA manipulator using KDL, TRAC-IK, and the proposed algorithm, respectively. The $EE$ of the KUKA manipulator must trace the given circle and square paths repeatedly, where the radius of the circle is $0.14$ m and the side length of the square is $0.2$ m. Both paths have been interpolated to 100 path points. Figure 2 illustrates the joint trajectories synthesized by the above three algorithms when tracing the given circular (Fig. 3(a)) and square (Fig. 3(c)) paths twice, in which the initial joint angles are ${\boldsymbol{\Theta }_{init}}$ $ = [0.526$, $ -0.609$, $ 0$, $ -1.431$, $ 0$, $ - 1.102$, $0.526]$ rad and ${\boldsymbol{\Theta }_{init}}$ $= [0.777$, $ - 0.888$, $ 0$, $ -0.936$, $ 0$, $ -1.316$, $ 0.777]$ rad, respectively. As shown in Fig. 2(a), (b), (d), and (e), the drifts of joints 1, 3, 5, and 7 can be clearly observed when using KDL and TRAC-IK. Contrastively, the joint trajectories deduced by the proposed algorithm all return to their initial states in Fig. 2(c) and (f), which indicates the JDF motion is realized. Moreover, Fig. 3(a) and (c) illustrate the tracking performance of the proposed algorithm, in which the $ EE$ can successfully trace the specified paths (red lines) with good continuity of configurations (blue lines). Using Eq. (40) to generate variation curves of the position error and pose error, it can be observed from Fig. 3(b) and (d) that the maximum value of position error is less than $ 10^{-6}$ and the pose error is always zero, both of which meet the error constraint and demonstrate the theoretical analyses in Section 2.1.

Figure 3. (a), (c) Manipulator motion trajectory profiles and the (b), (d) Cartesian errors generated by the proposed algorithm when tracing the given circular (first row) and square (second row) paths twice.

Figure 4. The configurations of the KUKA manipulator synthesized by (a), (d) KDL, (b), (e) TRAC-IK, and (c), (f) the proposed algorithm, respectively, when passing the same point in the 20-cycle circular and square tracing tasks.

As the tip of the KUKA manipulator continues to trace the paths, some of the joint angles drift to their fixed values until the path is closed in C-space when using KDL and TRAC-IK. Figure 4 displays the whole joint-drift process induced by KDL and TRAC-IK, and the joint angle repeatability guaranteed by using the proposed algorithm. Table III lists the detailed simulation data, in which the joint-drift ($\left \|{\boldsymbol{\Theta } \!\left ({20} \right ) - \boldsymbol{\Theta } \!\left ( 0 \right )} \right \|_{2}$) synthesized by the proposed algorithm in different tracing tasks is $4.491 \times{10^{-7}}$ and $ 9.177 \times 10^{-8}$ rad, which are significantly smaller than those of KDL and TRAC-IK. For the circular and square tracing tasks with 2000 path points, the average solution time of the proposed algorithm is $ 0.243$ and $ 0.239$ ms, respectively. Notably, the JDF motion is realized by the proposed algorithm with only three parameters, that is, ${{\textbf{q}}_{0}}$, ${\textbf{P}}_{nt}$, and Cartesian error requirement, which omit the time-consuming tuning process. In summary, the simulation results substantiate the effectiveness of the proposed algorithm when applied to the KUKA manipulator to perform real-time JDF motion.

Table III. Comparisons of joint-drifts among KDL, TRAC-IK, and the proposed algorithm when the tip traces the two different paths twenty times.

Finally, since the proposed algorithm does not induce pose error, the effects of position error on joint-drift (i.e., $\left \|{\boldsymbol{\Theta } \!\left ({20} \right ) - \boldsymbol{\Theta } \!\left ( 0 \right )} \right \|_{2}$) are verified. As shown in Table IV, position error causes a minor change in joint-drift within the computation accuracy. This effect is insignificant when compared to the change in magnitude of the position error.

Table IV. The effects of position error on joint-drift when the proposed algorithm is applied to different tracing tasks.

Figure 5. Screwing simulation results using (a) the proposed algorithm, (b) TRAC-IK, and (c) KDL, respectively. For each row, the first column shows the initial and final configurations, and the second column shows the joint trajectories and the variations of $EE$ height relative to the base.

3.2. Screwing simulation

The 2F-85 ROBOTIQ gripper and KUKA manipulator are used to perform the screwing simulation. Meanwhile, the connector is designed to connect the KUKA flange and the ROBOTIQ gripper, as shown in Fig. 5(a). The initial joint angle vector is ${\boldsymbol{\Theta }_{init}} = [0.527, -0.609,0, -1.430, 0, -1.102, -3.0]$ rad. Due to the joint limit of $\theta _{7}$, the height of $EE$ needs to be adjusted to reset $\theta _{ 7}$ during the screwing process. Hence, the above process is divided into the following stages:

1. (1) $EE$ is first planned to finish the motion in which the gripper rotates and drops in height until reaching the limit of $\theta _{7}$.

2. (2) $EE$ moves up to reset $\theta _{7}$ and then returns to the height at the end of the first stage.

3. (3) Repeat the motion of the first stage.

To complete the solution for each path point, the timeout is $ 15$ ms and the running type of TRAC-IK is “Distance,” which means TRAC-IK runs the full timeout and then returns the solution that minimizes the sum of squared error from the seed. As shown in Fig. 5, the green dotted lines indicate the different stages, from which the variations of $EE$ height clearly show the screwing processes. In contrast with TRAC-IK and KDL, the proposed algorithm can rotate the 7th joint significantly as desired and with little variation in other joint angles during the process. Both KDL and TRAC-IK induce an intense swing of the elbow joint, which increases the risk of colliding with nearby objects and people. Moreover, for 664 path points, the average solution time of the proposed algorithm is $ 0.223$ ms.

3.3. Manipulability performance evaluation

In this subsection, the simulation path points in Cartesian space are obtained by calculating the linear path between the target configuration in C-space and the zero position using forward kinematics, where the target configuration is selected randomly in the low manipulability region of the KUKA manipulator. The average values of manipulability ${w_{avg}}$ are obtained by solving the path with the three algorithms and used as the measure indexes. Moreover, both the running types “Manip1” and “Distance” of TRAC-IK are used to solve the path, in which “Manip1” means TRAC-IK runs the full timeout and then returns the solution that maximizes $ w$. The timeout is $10$ ms to guarantee the solution stability of KDL and TRAC-IK. The target configuration is ${\boldsymbol{\Theta }_{end}} = [0.144, -0.027, 2.805, -0.869, 1.610, 0.112, 1.649]$ rad. As shown in Fig. 6(c), although the max $ w_{avg}$ is derived, the oscillatory joint trajectories are generated when using the “Manip1” type of TRAC-IK, which is not applicable to continuous motion. Figure 6(d) indicates that the proposed algorithm can generate smooth trajectories with a higher ${w_{avg} = 1.411 \times 10^{-3}}$, which is approximately triple that of KDL (Fig. 6(a)) and the “Distance” type of TRAC-IK (Fig. 6(b)). The average solution time of the proposed algorithm is $ 5.543$ ms, and the simulation results substantiate the above statements that the proposed algorithm can produce superior configurations relative to KDL and TRAC-IK.

Figure 6. Joint trajectories derived from manipulability simulations using (a) KDL, TRAC-IK with the running types (b) “Distance” and (c) “Manip1”, and (d) the proposed algorithm, respectively.

Figure 7. The experiments for tracking the circle (first three rows) and square (last three rows) paths are performed on the KUKA manipulator using (a), (d) KDL, (b), (e) TRAC-IK, and (c), (f) the proposed algorithm, respectively. The KUKA manipulator begins with the initial point (first column), moves counterclockwise through the path points (middle three columns), and finally gets back to the start point (last column).

4.1. Experiment A: Circular and square path tracking

In order to verify the reliability of joint-drift simulation results, the physical experiments are performed on the KUKA manipulator using the above three baselines. For the circular and square path tracing, the initial joint angles of the KUKA manipulator are ${\boldsymbol{\Theta } _{init}} = [0.527, -0.609, 0, 1.430, 0, -1.102, 0.527]$ rad and ${\boldsymbol{\Theta }_{init}} = [0.777, - 0.888,0, 0.936,0, -1.316,0.777]$ rad, respectively. The initial state for each iteration of the proposed algorithm is ${{\textbf{q}}_{0}} = [0,0,0,0,0,0,0]$ rad. As shown in Fig. 7(c) and (f), the tip of the KUKA manipulator can track the red paths accurately and steadily when using the proposed algorithm. Meanwhile, the continuity of configurations can be guaranteed. After 20-cycle tracking tasks, the initial and final configurations of the KUKA manipulator are identical. On the contrary, as shown in the last columns of Fig. 7(a), (b), (d), and (e), the JDP caused by KDL and TRAC-IK can be clearly observed. Thus, the feasibility of the proposed algorithm to generate JDF motion with guaranteed Cartesian precision is experimentally verified. All the experimental processes can be found in the accompanying video (See Supplementary materials).

4.2. Experiment B: Screwing assembling

This subsection presents the screwing experiment performed by the KUKA manipulator and 2F-85 ROBOTIQ gripper to validate the simulation results derived from the proposed algorithm. The initial joint angles of the KUKA manipulator are ${\boldsymbol{\Theta }_{init}} = [0.527, -0.609,0, 1.430, 0, -1.102, -3.0]$ rad, and the initial state for each iteration of the proposed algorithm is ${{\textbf{q}}_{0}} = [0,0,0,0,0,0,0]$ rad. In this experiment, the allowable motion range of the seventh joint is set from $ -3$ to $ 1.714$ rad. As shown in Fig. 8, the screwing experiment is divided into four phases. When the seventh joint reaches $ 1.714$ rad in the second and fourth columns of Fig. 8, the $ EE$ should be raised to reset the seventh joint and the gripper should be released. The KUKA manipulator finished the whole screwing process with minor variations in other joints and no swing of the elbow, which is presented in detail in the accompanying video (See Supplementary materials). In addition, it can be observed that the bolt can be accurately screwed into the hole with the guarantee of $ EE$ precision by the proposed algorithm.