# Solution to the Inverse Problem of the Angular Manipulator Kinematics with Six Degrees of Freedom
## 2 Methods and Algorithms
### Determination of Spherical Coordinates of the Tool  
The position of the tool, as a rigid body, is completely determined in space by three points that do not lie on the same straight line, for example, \(D\), \(E\), \(K\) (see Figures 1 and 2). We will assume that the position of these characteristic points forming a rigid triangle is known from the working process conditions or is somehow determined applying the Euler angles or quaternions. It is convenient to represent the orientation of the main and backup CSs introduced above by three angles; two of them are the angles of the geographic CS (latitude \(\lambda\), longitude \(\varphi\)) and the third is an additional angle \(\psi\) of its own rotation around the abscissa axis. Thus, the main CS has the letter \(E\) as an index in the designation of the axes and indicated angles,  
Figure 2: Coordinate systems of tool angles. and the backup one has the letter \(K\) (see Figure 2, where these CSs are indicated in green and blue). Determination of the Geographical Coordinates of Points E and K  
Let us find the coordinates of points \(E\) and \(K\) in the CS _Dxyz_ (see Figure 2), associated with point \(D\) and axes parallel and co-directional to the axes of the absolute CS _OXYZ_ (see Figure 1) as follows  
\[ \begin{array}{ll}x_{E}=X_{E}-X_{D},&y_{E}=Y_{E}-Y_{D},&z_{E}=Z_{E}-Z_{D},\\ x_{K}=X_{K}-X_{D},&y_{K}=Y_{K}-Y_{D},&z_{K}=Z_{K}-Z_{D}.\end{array}  \tag{1}\]  
Then, the geographical coordinates of these points of the working tool are determined as follows  
\[\text{longitude: }\varphi_{E}=\arcsin\Big{(}\frac{z_{E}}{b}\Big{)},\quad \varphi_{K}=\arcsin\Big{(}\frac{z_{K}}{c}\Big{)} \tag{2}\]  
\[\text{latitude: }\lambda_{E}=\text{atan2}(y_{E},x_{E}),\ \lambda_{K}=\text{atan2}(y_{K},x_{K}), \tag{3}\]  
where \(c=DK=\sqrt{a^{2}+b^{2}+2 ab\cos\
u}\), \(a,b\) are the dimensions \(EK\) and \(ED\), respectively, and \(\
u\) is the tool bend angle. Due to the need to determine angles \(\lambda_{E},\lambda_{K}\) throughout the entire rotation, here and below, the additional function \(\text{atan2}(y,x)\) is introduced. It provides expressions for \(\arctan\big{(}y/x\big{)}\) but calculates angle values in the range \((-\pi,\,\pi]\). Unlike Equation (2), Equation (3) can degenerate when one of the points \(E\) or \(K\) is situated on the same vertical with point \(D\); therefore, the values of the coordinates \(x_{E},y_{E},\)\(x_{K},\)\(y_{K}\) are much better than the values  
\[\cos\varphi_{E}=\frac{\sqrt{x_{E}^{2}+y_{E}^{2}}}{b},\quad\cos\varphi_{K}= \frac{\sqrt{x_{K}^{2}+y_{K}^{2}}}{b}\]  
which provide an indicator of which of the CSs, the main one or the backup one, must be used in a given position of the mechanism for the consistency of the algorithm. The proximity to zero of one of the indicated cosines indicates the need to use an alternative coordinate system. The angle \(\psi_{E}\) of the CS's own rotation around the axis (see Figure 2) can be found from the condition that point \(K\) is located in the plane \(Dx^{(E)}y^{(E)}\). To do this, you need to find the cosines and sines of these angles. This can be conducted most conveniently by calculating the coordinates and point \(K\), which will be obtained as a result of two successive rotations of the CS _Dxyz_ by angles \(\lambda_{E}\) and \(-\varphi_{E}\). The resulting system, the position of point \(K\) and the angle \(\psi_{E}\) are shown in Figure 3.  
The rotation matrix for determining these coordinates through the coordinates (1) of point \(K\) is represented by the following expression  
\[\mathbf{S}(\lambda_{E},\varphi_{E})= \begin{bmatrix}\cos\varphi_{E}\cos\lambda _{E}&\cos\varphi_{E}\sin\lambda_{E}&\sin\varphi_{E}\\ -\sin\lambda_{E}&\cos\lambda_{E}&0\\ -\sin\varphi_{E}\cos\lambda_{E}&-\sin\varphi_{E}\sin\lambda_{E}&\cos\varphi_ {E}\end{bmatrix} \]  
The required coordinates are computed as follows  
\[ \begin{array}{l}y_{K}^{(2)}=-x_{K}\sin\lambda_{E}+y_{K}\cos\lambda_{E},\\ z_{K}^{(2)}=-x_{K}\sin\varphi_{E}\cos\lambda_{E}-y_{K}\sin\varphi_{E}\sin \lambda_{E}+z_{K}\cos\varphi_{E}.\end{array}