Inverse Kinematics of a Class of 6R Collaborative Robots with Non-Spherical Wrist

Abstract

The spread of cobotsin common industrial practice has led constructors to prefer the development of collaborative features that are necessary to prevent injuries to operators over the realization of simple kinematic structures for which the joints-to-workspace mapping is well known. An example is given by the replacement in serial robots of spherical wrists with safer solutions, where the danger of crushing and shearing is intrinsically avoided. Despite this tendency, the kinematic map between actuated joints and the Cartesian workspace remains of paramount importance for robot analysis and programming, deserving the attention of the research community. This paper proposes a closed-form solution for the inverse kinematics of a class of 6R robotic arms with six degrees of freedom and non-spherical wrists. The solutions are worked out by a single polynomial, of minimum degree, in terms of one of the positioning parameters chosen for the description of the robot posture. The roots of such a polynomial are then back-substituted to determine all the remaining unknowns. A numerical example is finally shown to verify the validity of the proposed implementation for a commercial collaborative robot.

1. Introduction

The last decade has witnessed the spread of collaborative robotics in many aspects of everyday life, from common industrial practice [1] to home assistance and healthcare [2,3], and service robotics in general [4]. The main reason for such an impact lies in the flexible use of cobots (collaborative robots). Their success in industry, in fact, is mostly attributed to the possibility to implement workflows where humans and robots safely cooperate in a shared environment [5,6,7]. This feature has recently being exploited in fields of applications different from bare production, where the use of classical industrial robots would be impossible for obvious safety reasons.

From the point of view of robot producers, such an unprecedented diffusion has required the development of innovative safety features, aimed at making the use of cobots increasingly secure and accessible to the wider public. Therefore, on the one hand, producers have been required to implement expensive sensors and use appropriate materials to assess the risk of injury in using cobots, and, on the other, they have been forced to maintain competitive production costs to make their machines accessible. The first consequence, as visible in many well-known commercial cobots, has been the adoption of serial kinematic structures that are simple and inexpensive to realize, such as those characterized by non-serial wrists.

The class of collaborative robots produced by FANUC, among others, belongs to such a category. Their kinematic structure, described in the following in detail, is characterized by a non-spherical roll–pitch–roll wrist that makes inverse kinematics mapping quite challenging. Recent literature shows different approaches to the issue. Trinh et al. [8] proposed a geometrical approach consisting of the solution of four separate univariate polynomials. In [9], the authors proposed a solution via numerical optimization of a problem formulated by means of polynomials for a similar robot (namely the Kinova Gen-3 Lite). In [10], a solution for a different non-spherical collaborative robot is tackled as well, although the wrist topology in this case allows a far simpler approach based on the computation of the first joint rotation at the very first step of the algorithm.

In less recent years, the problem has been approached in more general ways. Among others, Raghavan and Roth approached the problem of general linkages [11,12], demonstrating the existence of a maximum number of 16 solutions for the inverse kinematics of 6R (i.e., mechanisms owning a kinematic chain of six revolute joints) linkages. Their work, based on the solution via dialytic elimination of joints variables, inspired many other polynomial approaches, such as the Groebner basis work proposed by Wang et al. [13], or the eigenvalue approaches of Fu et al. [14] and Ghazvini [15].

Aside from such general solutions, the research community has provided many others for manipulators with spherical wrists. Among them, the method proposed by Xiao et al. [16] proposed to reach a solution by applying two cutting points within the kinematic chain and comparing the equations coming from their re-connection. Li et al. [17] considered the effect of compensation of the link lengths, so that their approach became effective after kinematic calibration.

Of course, the literature also offers plenty of numerical approaches to the problem, commonly used to find a solution by letting a starting configuration converge towards an end pose. Such methods are often based on the robot Jacobian matrix [18], differing one from the other for the management of singularities within the solution path. Among them, the Damped Least-Squares is the iterative approach most widely used [19,20,21,22], although convergence is strictly dependent on the used damping factor. Other more recent approaches have also been experimented with, such as the training of specific artificial intelligence as done in [23]. In any case, all of these approaches only allow the discovery of one solution to the problem (the closest to a first guess, usually), disregarding the large number of postures a 6R kinematic chain can exhibit to reach a given pose.

To overcome the limits of numerical approaches, a specific closed-form solution to the inverse kinematics of the FANUC CRX family of collaborative robots is proposed in this manuscript. As discussed in the following, the solution is worked out in the form of a univariate polynomial in one of the orientation parameters of the first body of the wrist. Such an approach allows the obtaining of the solving polynomial (of degree 16) with relative ease, starting from a system of six constraint equations. The backwards substitution needed for the computation of the remainder of the unknowns is then described in detail. Finally, a numerical example to verify the correctness of the solution is also proposed.

2. Robot Description

A robot of great interest in the collaborative robotics scene is the FANUC CRX-10iA/L (see Figure 1a), which meets industrial reliability requirements and provides all the necessary functionality for human–robot collaboration. In this paper, it is taken as a reference from the family of serial robots with non-spherical wrists, without loss of generality. The kinematic chain of the FANUC CRX-10iA/L is made of six revolute joints (6R) arranged in serial configuration. The first three joints (${\mathrm{J}}_1$, ${\mathrm{J}}_2$ and ${\mathrm{J}}_3$ in Figure 1b) are arranged in a classic kinematic sequence typical of robotic arms while the roll–pitch–roll wrist has a non-spherical configuration. The last three joints (${\mathrm{J}}_4$, ${\mathrm{J}}_5$ and ${\mathrm{J}}_6$), in fact, have axes that are two-by-two perpendicular and incident. Moreover, the lack of a common intersection point obviously makes the wrist non-spherical, making it impossible to approach the inverse kinematic problem with the usual methods for serial anthropomorphic manipulators, such as the solution proposed by Pieper [24].

The position and orientation of robot bodies in space are described here with the usual Denavit–Hartenberg notation. It is worth remembering that the mutual position between two frames can be represented by a homogeneous transformation, whose expression is given by:

$${}^i{\mathbf{T}}_j=\left[\begin{array}{cccc}cos{\theta }_i& -sin{\theta }_i& 0& 0\\ sin{\theta }_i& cos{\theta }_i& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& cos{\alpha }_i& -sin{\alpha }_i& 0\\ 0& sin{\alpha }_i& cos{\alpha }_i& 0\\ 0& 0& 0& 1\end{array}\right]$$

(1)

where ${\theta }_i,d_i,{\alpha }_i,a_i$ are the well-known Denavit–Hartenberg parameters. In the present case, the actuated joint variables $q_i$ are the rotations ${\theta }_i$ around the local z-axes.

The FANUC CRX-10iA/L frames are arranged as shown in Figure 2a, while the parameters collected in

Table 1. Denavit–Hartenberg parameters of the FANUC CRX-10iA/L.

i	${\mathit{\alpha }}_{\mathit{i}}$ (rad)	${\mathit{a}}_{\mathit{i}}$ (mm)	${\mathit{d}}_{\mathit{i}}$ (mm)	${\mathit{\theta }}_{\mathit{i}}$ (rad)	Motion Range (rad)
1	$\pi /2$	0	250.3	$q_1$	$\pm \pi$
2	$-\pi$	710.0	260.4	$q_2$	$\pm \pi$
3	$-\pi /2$	0	260.4	$q_3$	$\pm 1.5\pi$
4	$-\pi /2$	0	540.0	$q_4$	$\pm 1.06\pi$
5	$\pi /2$	0	150.0	$q_5$	$\pm \pi$
6	0	0	160.0	$q_6$	$\pm 1.25\pi$

complete their description. The configuration in space of the tool frame $\left\{6\right\}$ with respect to the global reference frame $\left\{0\right\}$ is obtained by composing the local transformations ${}^i{\mathbf{T}}_j$ according to the convention of successive transformation on mobile axes:

$${}^0{\mathbf{T}}_6=\prod _{i=1}^6{}^{i-1}{\mathbf{T}}_i={}^0{\mathbf{T}}_1{}^1{\mathbf{T}}_2{}^2{\mathbf{T}}_3{}^3{\mathbf{T}}_4{}^4{\mathbf{T}}_5{}^5{\mathbf{T}}_6$$

(2)

3. Inverse Kinematics Problem

As is well known, the inverse kinematics mapping of a serial chain consists of determining the joint parameters ($q_i$ in the previous notation) able to provide a given pose for the terminal body of the chain (i.e., body 6 in this manuscript, described by frame $\left\{6\right\}$). It is also well known that such a problem has many real solutions corresponding to the many postures that the robot can exhibit while reaching a given position and orientation of the terminal body.

A usual approach to this problem consists of simply comparing the elements of the final transformation ${}^0{\mathbf{T}}_6$ with a symbolic expression of the same matrix obtained considering unknown the six joint parameters $q_i$:

$$\begin{array}{ccc}{}^0{\mathbf{T}}_6& \underset{kinematics}{\overset{inverse}{\to }}& \mathbf{q}=\left[\begin{array}{cccccc}{q}_1& q_2& q_3& q_4& q_5& q_6\end{array}\right]\end{array}$$

(3)

This allows the obtaining of a set of 12 equations that can be reduced to six considering that only three elements of the rotational part of the homogeneous transformation matrix are actually independent. Additionally, a common issue in approaching this kind of equation is given by the presence of sines and cosines, which add complexity to the problem. The usual solution is to apply the Weierstrass substitution to have a common parameterization without trigonometric functions, or to treat them (in this case, 12 expressions of $cos{q}_i$ and $sin{q}_i$ for $i=1,\cdots ,6$ ) as distinct unknowns related by six further homogeneous equations (${cos}^2{q}_i+{sin}^2{q}_i-1=0$). Both strategies lead to a set of polynomial equations of degree 2, whose solution yields to a univariate polynomial: nonetheless, the reduction of such a system is often computationally burdening and the possibility of obtaining a final degree higher than the number of expected results is also significant.

The approach proposed in this manuscript takes into account a different set of unknown variables, hereby collected in a vector called $\mathbf{p}$, which locates the reference frame of one of the robot bodies (frame $\left\{4\right\}$ in particular) and, as a result, allows the computation of the coordinates in space of the robot nodal points ${\mathbf{P}}_i$ (shown in Figure 2). From such points, then, the computation of the joint variables becomes a trivial issue.

In more detail, the parameterization chosen to express the rotation matrix ${}^0{\mathbf{R}}_4$ between frame $\left\{4\right\}$ and the fixed frame $\left\{0\right\}$ avoiding trigonometric functions is the Cayley transform, which maps the skew–symmetric matrix $\mathbf{C}$ given by:

$$\mathbf{C}=\left[\begin{array}{ccc}0& -c_3& c_2\\ c_3& 0& -c_1\\ -c_2& c_1& 0\end{array}\right]$$

(4)

into a rotation matrix:

$${}^0{\mathbf{R}}_4=\left(\mathbf{I}+\mathbf{C}\right){\left(\mathbf{I}-\mathbf{C}\right)}^{-1}$$

(5)

Therefore, the shape assumed by the resulting rotation matrix ${}^0{\mathbf{R}}_4$ in terms of the three parameters $c_i$ (with $i=1,2,3$) is:

$${}^0{\mathbf{R}}_4=\frac{1}{c_1^2+c_2^2+c_3^2+1}\left[\begin{array}{ccc}{c}_1^2-c_2^2-c_3^2+1& 2c_1{c}_2-2c_3& 2c_2+2c_1{c}_3\\ 2c_3+2c_1{c}_2& -c_1^2+c_2^2-c_3^2+1& 2c_2{c}_3-2c_1\\ 2c_1{c}_3-2c_2& 2c_1+2c_2{c}_3& -c_1^2-c_2^2+c_3^2+1\end{array}\right]$$

(6)

Based on such a choice, the Inverse Kinematics Problem actually consists of finding the parameters that allow the determination of the pose of $\left\{4\right\}$ (expressed by means of $c_1$, $c_2$, $c_3$ and coordinates of ${\mathbf{P}}_4$) starting from $\left\{6\right\}$, and it can be stated as:

$$\begin{array}{ccc}{}^0{\mathbf{T}}_6& \underset{kinematics}{\overset{inverse}{\to }}& \mathbf{p}=\left[\begin{array}{cccccc}{c}_1& c_2& c_3& P_{4,x}& P_{4,y}& P_{4,z}\end{array}\right]\end{array}$$

(7)

where the variables $c_i$ can be used to describe the orientation of frame $\left\{4\right\}$, as mentioned above, and $P_{4,x}$, $P_{4,y}$, $P_{4,z}$ are the coordinates of its origin, gathered in the column vector ${\mathbf{P}}_4$. The 6 parameters are then used to build up the needed transformation matrix:

$${}^0{\mathbf{T}}_4=\left[\begin{array}{cc}{}^0{\mathbf{R}}_4& {\mathbf{P}}_4\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]$$

(8)

with obvious meaning of the terms involved. In the following, the system of equations used to work out the solution of the kinematic problem is shown in detail together with its solution path.

3.1. System of Equations

To find the configuration of frame $\left\{4\right\}$ once given a value for ${}^0{\mathbf{R}}_6$, a set of six equations must be provided. To achieve such an aim, it is possible to exploit both the loop-closure of the kinematic chain and the mobility provided by the joint topology. The former can be obtained by considering as known the position in space of points ${\mathbf{P}}_5$ and ${\mathbf{P}}_6$, while the latter is achieved through the mobility of the non-spherical wrist and the anthropomorphic arm.

Starting from the kinematic closure, the coordinates of ${\mathbf{P}}_5$ are easily obtained via:

$$\left[\begin{array}{c}{\mathbf{P}}_5\\ 1\end{array}\right]={}^0{\mathbf{T}}_6\left[\begin{array}{c}0\\ 0\\ -d_6\\ 1\end{array}\right]=\left[\begin{array}{c}{P}_{5,x}\\ P_{5,y}\\ P_{5,z}\\ 1\end{array}\right]$$

(9)

The same point can be written starting from $\left\{4\right\}$ in terms of the problem unknowns, namely:

$$\left[\begin{array}{c}{\mathbf{P}}_5\\ 1\end{array}\right]={}^0{\mathbf{T}}_4\left[\begin{array}{c}0\\ 0\\ d_5\\ 1\end{array}\right]$$

(10)

By comparing (9) and (10), the first three equations are obtained. After simplification and elimination of the non-vanishing terms, the three equations obtained are three polynomials in terms of the six unknowns collected in $\mathbf{p}$:

$$\begin{array}{cc}{\mathsf{\Phi }}_1:\hfill & (P_{4,x}-P_{5,x})(c_1^2+c_2^2+c_3^2+1)+2d_5(c_1{c}_3+c_2)=0\hfill \\ {\mathsf{\Phi }}_2:\hfill & (P_{4,y}-P_{5,y})(c_1^2+c_2^2+c_3^2+1)+2d_5(c_2{c}_3-c_1)=0\hfill \\ {\mathsf{\Phi }}_3:\hfill & (P_{4,z}-P_{5,z})(c_1^2+c_2^2+c_3^2+1)-d_5(c_1^2+c_2^2-c_3^2-1)=0\hfill \end{array}$$

(11)

Three additional equations can be worked out from the mobility of frame $\left\{4\right\}$. First, it can be noticed that the $y_4$-axis of the frame is constrained to intersect the $z_0$-axis of the global coordinate system (since $d_2=d_3$), except for the case where the two axes are parallel and not coincident, i.e., when the $y_4$-axis is directed along vertical and does not pass through the origin of the fixed frame. The latter condition, by the way, is far from the operating conditions of the robot. A more general condition can be stated as the coplanarity of the four points ${\mathbf{P}}_0$, ${\mathbf{P}}_1$, ${\mathbf{P}}_3$ and ${\mathbf{P}}_4$ or better as the linear dependency among the three vectors ${\mathbf{z}}_0$, ${\mathbf{y}}_4$ and $({\mathbf{P}}_4-{\mathbf{P}}_0)$. Such a condition is fulfilled when:

$$det\left[\begin{array}{ccc}{\mathbf{z}}_0& {\mathbf{y}}_4& ({\mathbf{P}}_4-{\mathbf{P}}_0)\end{array}\right]=0$$

(12)

where ${\mathbf{z}}_0={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$, ${\mathbf{y}}_4={}^0{\mathbf{R}}_4{\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T$ and $({\mathbf{P}}_4-{\mathbf{P}}_0)={\left[\begin{array}{ccc}{P}_{4,x}& P_{4,y}& P_{4,z}\end{array}\right]}^T$. After some manipulation and elimination of non-vanishing components, the determinant in (12) can be expanded to obtain the fourth equation of the system:

$$\begin{array}{cc}{\mathsf{\Phi }}_4:\hfill & (c_1^2-c_2^2+c_3^2-1)P_{4,x}+(2c_1{c}_2-2c_3)P_{4,y}=0\hfill \end{array}$$

(13)

The mobility of the kinematic chain, of the wrist in particular, also constrains frame $\left\{4\right\}$ to maintain an axis perpendicular to the assigned coordinate system $\left\{6\right\}$, i.e., the axes identified by vectors ${\mathbf{z}}_4$ and ${\mathbf{z}}_6$. The perpendicularity is fulfilled when:

$${\mathbf{z}}_4^T{\mathbf{z}}_6=0$$

(14)

where ${\mathbf{z}}_4={}^0{\mathbf{R}}_4{\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$ and ${\mathbf{z}}_6$ is the unit vector of $({\mathbf{P}}_5-{\mathbf{P}}_6)$ (for the constraint purpose, they can be used interchangeably). After substitution and simplification, the following polynomial is obtained:

$$\begin{array}{cc}{\mathsf{\Phi }}_5:\hfill & 2(c_1{c}_3+c_2)(P_{5,x}-P_{6,x})+2(c_2{c}_3-c_1)(P_{5,y}-P_{6,y})+\hfill \\ \hfill & -(c_1^2+c_2^2-c_3^2-1)(P_{5,z}-P_{6,z})=0\hfill \end{array}$$

(15)

Finally, it must be noted that point ${\mathbf{P}}_3$ maintains a constant distance with respect to the center of the robot shoulder, i.e., to the point ${\mathbf{P}}_1$. Thus, it is:

$${({\mathbf{P}}_3-{\mathbf{P}}_1)}^T({\mathbf{P}}_3-{\mathbf{P}}_1)-a_2^2=0$$

(16)

where ${\mathbf{P}}_3={\mathbf{P}}_4+{}^0{\mathbf{R}}_4{\left[\begin{array}{ccc}0& d_4& 0\end{array}\right]}^T$ and ${\mathbf{P}}_1={\left[\begin{array}{ccc}0& 0& d_1\end{array}\right]}^T$, yielding the simplified form:

$$\begin{array}{cc}{\mathsf{\Phi }}_6:\hfill & {\left(P_{4,x}\left(c_1^2+c_2^2+c_3^2+1\right)+2d_4\left(c_1{c}_2-c_3\right)\right)}^2+\hfill \\ \hfill & +{\left(P_{4,y}\left(c_1^2+c_2^2+c_3^2+1\right)+d_4\left(c_1^2-c_2^2+c_3^2-1\right)\right)}^2+\hfill \\ \hfill & +{\left(\left(P_{4,z}-d_1\right)\left(c_1^2+c_2^2+c_3^2+1\right)+2d_4\left(c_2{c}_3+c_1\right)\right)}^2-a_2^2=0\hfill \end{array}$$

(17)

The six polynomial equations ${\mathsf{\Phi }}_i$ must now be solved in the unknown variables of $\mathbf{p}$ to obtain a single polynomial in terms of $c_1$ only. The solution path is detailed in the following section.

3.2. System Solution

At first, it is possible to remove the three Cartesian variables $P_{4,x}$, $P_{4,y}$ and $P_{4,z}$ exploiting the three equations ${\mathsf{\Phi }}_1$, ${\mathsf{\Phi }}_2$ and ${\mathsf{\Phi }}_3$ in which they only appear in linear form. Thus, the following expressions can be found in terms of $c_1$, $c_2$ and $c_3$:

$$\begin{array}{c}{P}_{4,x}(c_1,c_2,c_3)=P_{5,x}-d_5{\displaystyle \frac{2(c_1{c}_3+c_2)}{c_1^2+c_2^2+c_3^2+1}}\hfill \\ P_{4,y}(c_1,c_2,c_3)=P_{5,y}-d_5{\displaystyle \frac{2(c_2{c}_3-c_1)}{c_1^2+c_2^2+c_3^2+1}}\hfill \\ P_{4,z}(c_1,c_2,c_3)=P_{5,z}+d_5{\displaystyle \frac{c_1^2+c_2^2-c_3^2-1}{c_1^2+c_2^2+c_3^2+1}}\hfill \end{array}$$

(18)

Substitution of (18) into ${\mathsf{\Phi }}_4$, ${\mathsf{\Phi }}_5$ and ${\mathsf{\Phi }}_6$ yields to a formulation of the robot kinematics in terms of only $c_1$, $c_2$ and $c_3$. The resulting polynomials own a maximum degree of 2 and can be expressed as:

$${\mathsf{\Phi }}_h:\sum _{i,j,k=0}^{i+j+k\le 2}{\phi }_{h,ijk}{c}_1^i{c}_2^j{c}_3^k=0$$

(19)

where the coefficients ${\phi }_{h,ijk}$ are function of the robot geometric parameters and $h=4,5,6$. Appendix A shows in detail the value of such coefficients.

To reduce the system of equations and obtain a polynomial in terms of just one of the three unknowns (namely $c_1$), the Equation (19) can be rewritten consequently. To remove $c_3$ at first, the following shape can be worked out:

$$X_h:\sum _{k=0}^2{\chi }_{h,k}{c}_3^k=0$$

(20)

where the coefficients ${\chi }_{h,k}$ (detailed in Appendix B) are functions of $c_1$ and $c_2$. It is worth remarking that the formulations (19) and (20) coincide, thus ${\mathsf{\Phi }}_h=X_h$. However, polynomials (20) can be used to remove $c_3$ by means of two Sylvester matrices, whose determinants provide two further reduced equations:

$$\begin{array}{c}{\mathsf{\Psi }}_1:det\left[\begin{array}{cccc}{\chi }_{4,2}& {\chi }_{4,1}& {\chi }_{4,0}& 0\\ 0& {\chi }_{4,2}& {\chi }_{4,1}& {\chi }_{4,0}\\ {\chi }_{6,2}& {\chi }_{6,1}& {\chi }_{6,0}& 0\\ 0& {\chi }_{6,2}& {\chi }_{6,1}& {\chi }_{6,0}\end{array}\right]=0\\ {\mathsf{\Psi }}_2:det\left[\begin{array}{cccc}{\chi }_{5,2}& {\chi }_{5,1}& {\chi }_{5,0}& 0\\ 0& {\chi }_{5,2}& {\chi }_{5,1}& {\chi }_{5,0}\\ {\chi }_{6,2}& {\chi }_{6,1}& {\chi }_{6,0}& 0\\ 0& {\chi }_{6,2}& {\chi }_{6,1}& {\chi }_{6,0}\end{array}\right]=0\end{array}$$

(21)

The vanishing set of the determinants ${\mathsf{\Psi }}_1$ and ${\mathsf{\Psi }}_2$ (which are polynomials of $c_1$ and $c_2$) represent the solution to the inverse kinematics problem. The maximum degree of the polynomials is 4 and in compact form they can be formulated as:

$${\mathsf{\Psi }}_h:\sum _{i,j=0}^{i+j\le 4}{\psi }_{h,ij}{c}_1^i{c}_2^j=0$$

(22)

The elimination via Sylvester matrix can be adopted also to remove the unknown $c_2$ by rewriting (22) as:

$${\mathrm{\Omega }}_h:\sum _{j=0}^4{\omega }_{h,j}{c}_2^j=0$$

(23)

Again, it is remarked that (23) is a different formulation of (22), nevertheless ${\mathsf{\Psi }}_h={\mathrm{\Omega }}_h$. A complete formulation of coefficients ${\omega }_{h,j}$ is provided in Appendix C, where they are presented already as polynomials of the only unknown $c_1$.

The last step of the solution is made using again the Sylvester method for reduction of variable $c_2$ by means of (23). A further matrix is built, whose determinant is a polynomial of degree 16 in terms of the only unknown $c_1$:

$$\mathsf{\Lambda }:det\left[\begin{array}{cccccccc}{\omega }_{1,4}& {\omega }_{1,3}& {\omega }_{1,2}& {\omega }_{1,1}& {\omega }_{1,0}& 0& 0& 0\\ 0& {\omega }_{1,4}& {\omega }_{1,3}& {\omega }_{1,2}& {\omega }_{1,1}& {\omega }_{1,0}& 0& 0\\ 0& 0& {\omega }_{1,4}& {\omega }_{1,3}& {\omega }_{1,2}& {\omega }_{1,1}& {\omega }_{1,0}& 0\\ 0& 0& 0& {\omega }_{1,4}& {\omega }_{1,3}& {\omega }_{1,2}& {\omega }_{1,1}& {\omega }_{1,0}\\ {\omega }_{2,4}& {\omega }_{2,3}& {\omega }_{2,2}& {\omega }_{2,1}& {\omega }_{2,0}& 0& 0& 0\\ 0& {\omega }_{2,4}& {\omega }_{2,3}& {\omega }_{2,2}& {\omega }_{2,1}& {\omega }_{2,0}& 0& 0\\ 0& 0& {\omega }_{2,4}& {\omega }_{2,3}& {\omega }_{2,2}& {\omega }_{2,1}& {\omega }_{2,0}& 0\\ 0& 0& 0& {\omega }_{2,4}& {\omega }_{2,3}& {\omega }_{2,2}& {\omega }_{2,1}& {\omega }_{2,0}\end{array}\right]=0$$

(24)

In more compact form, the solving equation $\mathsf{\Lambda }$ is:

$$\mathsf{\Lambda }:\sum _{i=0}^{16}{\lambda }_i{c}_1^i=0$$

(25)

where the coefficients ${\lambda }_i$ for $i=1,\dots ,16$ are finally functions of only geometric parameters. Unfortunately, explicit expressions for the coefficients are too large to be shown in this document. Nonetheless, Appendix D shows (25) in terms of the polynomials ${\omega }_{h,i}$ appearing in matrix (24). The polynomial provides up to 16 real solutions to the problem, corresponding to its roots. The number of solutions does not change depending on proximity to singular poses or joint limits. However, it obviously becomes equal to 0 for singular configurations (such as any point out of the robot workspace) for which only complex roots can be found (25). In the remainder of the manuscript, details are provided about the back-substitution of such values and the numeric implementation for the computation of the other variables of the problem.

4. Implementation and Verification

As aforementioned, the vanishing set of the polynomial (25) allows the discovery of up to 16 real solutions for the inverse kinematics problem, corresponding to the roots of $\mathsf{\Lambda }$. The expanded equations shown in the appendices provide an impression of the coefficient dimensions that cannot be further reduced. For this reason, the products of polynomials ${\omega }_{h,i}$ have not been made explicit during the implementation of the inverse kinematics algorithm. On the contrary, the coefficients of $\mathsf{\Lambda }$ have been obtained as a sum of the coefficients of each addendum appearing in Appendix D, and each of them was worked out via the convolution of the corresponding discrete sequence of coefficients.

As is known, in fact, given two polynomials $f\left(x\right)$ and $g\left(x\right)$ of degree m and n, respectively:

$$\begin{array}{ccc}f\left(x\right)=\sum _{i=0}^m{f}_i{x}^i& & g\left(x\right)=\sum _{i=0}^n{g}_i{x}^i\end{array}$$

(26)

the product $f\left(x\right)g\left(x\right)$ is a $m+n$ degree polynomial given by:

$$\begin{array}{ccccc}h\left(x\right)=f\left(x\right)g\left(x\right)=\sum _{i=0}^{m+n}{h}_i{x}^i& & \mathrm{with}& & h_i=\sum _{k=0}^i{f}_k{g}_{i-k}\end{array}$$

(27)

The definition of $h_i$ can be easily extended to a $-\infty ,+\infty$ sum, and then to a definition of the convolution of a discrete series. Therefore, the coefficients $h_i$ of polynomial $h\left(x\right)=f\left(x\right)g\left(x\right)$ are computable as the convolution $f_i⊛g_i$. Let us take, for example, the multiplication ${\omega }_{1,3}{\omega }_{2,2}$. The coefficients of the resulting polynomial will be provided by the convolution of the coefficients of the factors, thus:

$${\omega }_{1,3}{\omega }_{2,2}=\left(\left[\begin{array}{c}{B}_1,C_1\end{array}\right]\left[\begin{array}{c}{c}_1\\ 1\end{array}\right]\right)\left(\left[\begin{array}{c}{D}_2,E_2,F_2\end{array}\right]\left[\begin{array}{c}{c}_1^2\\ c_1\\ 1\end{array}\right]\right)=\left(\left[\begin{array}{c}{B}_1,C_1\end{array}\right]⊛\left[\begin{array}{c}{D}_2,E_2,F_2\end{array}\right]\right)\left[\begin{array}{c}{c}_1^3\\ c_1^2\\ c_1\\ 1\end{array}\right]$$

(28)

where expressions in terms of the robot geometric parameters are provided in the appendices for $A_i$, $B_i$, $C_i$, etc.

Using convolution rather than multiplication allows computation with relative ease of the coefficients of $\mathsf{\Lambda }$ and therefore its 16 roots. Then, the remaining unknowns can be computed following the solution path backwards and substituting the values obtained time by time. Starting from (24), a linear system of equation can be built as:

$$\left[\begin{array}{cccccc}{\omega }_{1,4}& {\omega }_{1,3}& {\omega }_{1,2}& {\omega }_{1,1}& {\omega }_{1,0}& 0\\ 0& {\omega }_{1,4}& {\omega }_{1,3}& {\omega }_{1,2}& {\omega }_{1,1}& {\omega }_{1,0}\\ 0& 0& {\omega }_{1,4}& {\omega }_{1,3}& {\omega }_{1,2}& {\omega }_{1,1}\\ {\omega }_{2,4}& {\omega }_{2,3}& {\omega }_{2,2}& {\omega }_{2,1}& {\omega }_{2,0}& 0\\ 0& {\omega }_{2,4}& {\omega }_{2,3}& {\omega }_{2,2}& {\omega }_{2,1}& {\omega }_{2,0}\\ 0& 0& {\omega }_{2,4}& {\omega }_{2,3}& {\omega }_{2,2}& {\omega }_{2,1}\end{array}\right]\left[\begin{array}{c}{c}_2^6\\ c_2^5\\ c_2^4\\ c_2^3\\ c_2^2\\ c_2\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ -{\omega }_{1,0}\\ 0\\ 0\\ -{\omega }_{2,0}\end{array}\right]$$

(29)

Substituting one at a time the 16 values of $c_1$ (corresponding to the roots of $\mathsf{\Lambda }$) to evaluate the polynomials ${\omega }_{h,i}$, the respective values of $c_2$ can be found from a solution of (29). As $c_1$ and $c_2$ are now known, the coefficients ${\chi }_{h,k}$ of (20) can be computed and used to obtain $c_3$ picking two equations from the three $X_h$. For example, it is possible to formulate the linear system:

$$\left[\begin{array}{cc}{\chi }_{4,2}& {\chi }_{4,1}\\ {\chi }_{5,2}& {\chi }_{5,1}\end{array}\right]\left[\begin{array}{c}{c}_3^2\\ c_3\end{array}\right]=\left[\begin{array}{c}-{\chi }_{4,0}\\ -{\chi }_{5,0}\end{array}\right]$$

(30)

Lastly, the three orientation parameters $c_1$, $c_2$ and $c_3$ can be substituted into (18) to obtain the coordinates $P_{4,x}$, $P_{4,y}$, $P_{4,z}$ of the origin of the reference frame $\left\{4\right\}$. This passage almost closes the inverse kinematics problem, although the last few steps of joint variables computation are still at stake. To provide a complete mapping ${}^0{\mathbf{T}}_6⟶\mathbf{q}$, the coordinates of the points ${\mathbf{P}}_4$ and ${\mathbf{P}}_3={\mathbf{P}}_4+{}^0{\mathbf{R}}_4{\left[\begin{array}{ccc}0& d_4& 0\end{array}\right]}^T$ can be exploited. The passages to obtain the values of the joint variables, which are quite trivial, are not shown here for the sake of conciseness, although their full formulation is shown in Appendix E. However, it is worth remarking that their values are calculated in cascade, following the order $q_2$, $q_1$, $q_3$, $q_4$, $q_5$, $q_6$.

It is also worth noting that the very first computation, i.e., $q_2=arcsin(P_{3,x}-P_{1,x})/a_2$, doubles the number of available solutions (being acceptable both $q_2$ and $\pi -q_2$). Actually, the solutions obtained considering the full domain of the function arcsin are already present within the set of points coordinates previously found. In practice, applying in sequence the mapping ${}^0{\mathbf{T}}_6⟶\mathbf{p}$ and $\mathbf{p}⟶\mathbf{q}$ duplicated solutions are added to the available ones (that can be easily eliminated).

Numerical Example

Finally, to verify the effectiveness of the proposed solution, a numerical example is shown. A random set of joint angles is picked to compute the robot forward kinematics by (2). In particular, in degrees:

$$\mathbf{q}={\left[\begin{array}{cccccc}{78}^{\circ }& {131}^{\circ }& {24}^{\circ }& {42}^{\circ }& -{60}^{\circ }& -{10}^{\circ }\end{array}\right]}^T$$

(31)

which provide the transformation matrix:

$${}^0{\mathbf{T}}_6=\left[\begin{array}{cccc}0.3363& 0.8387& -0.4283& 0.0571\\ 0.6182& 0.1464& 0.7722& 0.1786\\ 0.7104& -0.5245& -0.4693& 0.7677\\ 0& 0& 0& 1\end{array}\right]$$

(32)

The resulting known points useful for inverse kinematics computations and for results check are (in mm):

$$\begin{array}{ccc}{\mathbf{P}}_0=\left[\begin{array}{c}0.0\\ 0.0\\ 0.0\end{array}\right]& {\mathbf{P}}_1=\left[\begin{array}{c}0.0\\ 0.0\\ 250.3\end{array}\right]& {\mathbf{P}}_3=\left[\begin{array}{c}125.7\\ 55.0\\ 842.7\end{array}\right]\\ {\mathbf{P}}_4=\left[\begin{array}{c}0.0\\ 0.0\\ 250.3\end{array}\right]& {\mathbf{P}}_5=\left[\begin{array}{c}125.7\\ 55.0\\ 842.7\end{array}\right]& {\mathbf{P}}_6=\left[\begin{array}{c}57.1\\ 178.6\\ 767.7\end{array}\right]\end{array}$$

(33)

Such coordinates, together with the geometrical parameters already introduced in Table 1, can be substituted into the explicit expressions of the coefficients of polynomials ${\omega }_{h,i}$. Such polynomials allow computing, via convolution, the coefficients of $\mathsf{\Lambda }$, whose roots represent the values of $c_1$ which are the solution of the inverse kinematics problem. Substituting in cascade into (29), (30) and (18), it is possible to obtain the values of the remaining unknowns, which are shown in

Table 2. Solutions of the inverse kinematics for the given joint variables (31): eight real solutions for $c_1$ out of 16 roots of $\mathsf{\Lambda }$.

${\mathit{c}}_1$	${\mathit{c}}_2$	${\mathit{c}}_3$	${\mathit{P}}_{4,\mathit{x}}$	${\mathit{P}}_{4,\mathit{y}}$	${\mathit{P}}_{4,\mathit{z}}$
−13.105	−0.262	−22.831	−3.5	46.7	766.9
−9.673	1.672	−4.490	10.5	49.4	938.7
−7.730	−18.981	8.395	176.8	147.5	949.1
−7.603	0.844	−3.933	2.6	37.9	926.8
−6.135	0.372	−3.417	−1.2	26.1	917.3
−3.738	−0.223	−2.270	2.7	−8.0	901.1
−3.230	−0.296	−4.901	−5.4	15.5	781.5
−0.667 + 0.50i	–	–	–	–	–
−0.667 − 0.50i	–	–	–	–	–
−0.480	1.417	−0.041	−7.3	16.0	900.0
−0.233 + 0.01i	–	–	–	–	–
−0.233 − 0.01i	–	–	–	–	–
−0.088 + 0.04i	–	–	–	–	–
−0.088 − 0.04i	–	–	–	–	–
−0.041 + 0.02i	–	–	–	–	–
−0.041 − 0.02i	–	–	–	–	–

. As is shown, the polynomial $\mathsf{\Lambda }$ provides in this case only eight real solutions, for which it is possible to evaluate eight poses for the reference frame $\left\{4\right\}$.

The results offered by the algorithm in terms of pose of $\left\{4\right\}$ can be then mapped into sets of joint variables by means of the equations presented in Appendix D. As aforementioned, the eight real solutions of $c_1$ are doubled by the angle computations (see

Table 3. Computedjoints variables corresponding to the eight real roots of $\mathsf{\Lambda }$: total of 16 configurations, four of which repeated.

#	${\mathit{c}}_1$	${\mathit{q}}_1$	${\mathit{q}}_2$	${\mathit{q}}_3$	${\mathit{q}}_4$	${\mathit{q}}_5$	${\mathit{q}}_6$
1	−13.105	−30.30	33.99	136.01	143.27	−48.79	77.61
2		149.69	146.00	43.98	−36.72	−48.79	77.61
3	−9.673	−101.99	48.99	155.99	−138.00	−59.99	−9.99
4		78.00	131.00	24.00	42.00	−60.00	−10.00
5	−7.730	39.90	28.21	161.19	−75.16	116.22	−90.34
6		−140.09	151.78	18.80	104.83	116.22	−90.34
7	−7.603	−93.98	47.62	154.56	−144.11	−55.12	−2.97
8		86.01	132.37	25.43	35.88	−55.12	−2.97
9	−6.135	−93.98	47.62	154.56	−144.11	−55.12	−2.97
10		86.01	132.37	25.43	35.88	−55.12	−2.97
11	−3.738	−93.98	47.62	154.56	−144.11	−55.12	−2.97
12		86.01	132.37	25.43	35.88	−55.12	−2.97
13	−3.230	−29.41	33.90	135.67	142.25	−49.24	78.79
14		150.58	146.09	44.32	−37.74	−49.24	78.79
15	−0.480	114.69	42.07	151.46	−23.88	170.53	10.81
16		−65.30	137.92	28.53	156.11	170.53	10.81

), although four of the resulting configurations (shown in Figure 3) are repeated.

5. Conclusions

This manuscript proposes a closed-form solution for the inverse kinematics mapping of a class of 6R robotic manipulators, characterized by a non-spherical wrist. On the one hand, such a feature improves the robot mechanical simplicity and provides some advantage in terms of dexterity; on the other, it prevents the use of common methods developed for anthropomorphic arms. For this reason, a specific approach has been worked out based on the constraint equations that characterize the mobility of an inner body of the kinematic chain (i.e., the first body of the non-spherical wrist). The solution path eliminates one at a time five out of the six unknowns of the problem and provides a univariate polynomial of degree 16, whose roots represent the problem solution. Backward substitution and computation of the actuated joint angles are shown to close the solution path and provide a full map of the end-effector configurations space and the motors space. In conclusion, the manuscript provides a complete walk-through to compute all the possible postures that the FANUC CRX family of cobots can exhibit to reach a given pose. Obviously, the proposed method is effective also for all the manipulators sharing their joint topology with the FANUC CRX family, i.e., for those manipulators for which the written equations are valid. Future developments will involve the analysis of the robot workspace to also provide a complete map of robot workspace boundaries and inner singularities, the analysis of representation singularities, and the impact of the problem of numerical conditioning on the accuracy of the kinematics solution.