# Solution to the Inverse Problem of the Angular Manipulator Kinematics with Six Degrees of Freedom
## 2 Methods and Algorithms
### Determination of Orientation Angles
#### 2.4.1 Orientation of the Gripper (Working Tool)  
To obtain the orientation of the gripper (working tool), the elbow must be rotated through angles \(\varphi_{4}\), \(\varphi_{5}\) and \(\varphi_{6}\) at hinges \(B_{2}\) and twice at \(K\), see Figures 1 and 5. The CS of the gripper _Kxyz_ is indicated in blue in Figure 1. Its orientation in the absolute CS can be determined through the spherical coordinates \(\varepsilon\), \(\varphi_{E}\), \(\psi_{E}\) (or \(\lambda_{K}\), \(\varphi_{K}\), \(\psi_{K}\)) introduced above in Section 2.1, or maybe through the angles of rotation of the manipulator links in the hinges \(\varphi_{1}\), \(\varphi_{2}\), \(\varphi_{3}\), \(\varphi_{4}\), \(\varphi_{5}\), \(\varphi_{6}\). The matrix of rotations \(\mathbf{S}_{(Kxyz)}^{(OXYZ)}\) in absolute CS \(OXYZ\) for the CS of the gripper _Kxyz_ is represented in this case by the product of the rotation matrix from the CS \(OXYZ\) to CS \(B_{2}xyz\) of the elbow \(\left(\mathbf{S}_{(Bxyz)}^{(OXYZ)}\right)\) and rotation matrix from CS \(B_{2}xyz\) of elbow to the CS _Kxyz_ of the gripper \(\left(\mathbf{S}_{(Kxyz)}^{(B_{2}xyz)}\right)\) as follows:  
\[\mathbf{S}_{(Kxyz)}^{(OXYZ)}=\mathbf{S}_{(B_{2}xyz)}^{(OXYZ)}\mathbf{S}_{(Kxyz)} ^{(B_{2}xyz)} \tag{12}\]  
Here, the value of the rotation matrix from the CS \(OXYZ\) to the CS of the elbow \(B_{2}xyz\)\(\left(\mathbf{S}_{(Bxyz)}^{(OXYZ)}\right)\) has already been obtained and is given by Equations (5)-(9) and (11). Let us form a rotation matrix for the elbow CS \(B_{2}xyz\) to the gripper CS \(Kxyz\) using matrices of the sequence of rotations that are carried out with the following transition  
\[\mathbf{S}_{(Kxyz)}^{(B_{2}xyz)}=\mathbf{S}_{x}(\varphi_{4})\mathbf{S}_{y}( \varphi_{5})\mathbf{S}_{x}(\varphi_{6})\]  
In accordance with this, the above-mentioned matrix is obtained  
\[\mathbf{S}_{(Kxyz)}^{(B_{2}xyz)}\!=\! \begin{bmatrix}C_{\varphi_{5}}&S_{\varphi _{5}}S_{\varphi_{6}}&S_{\varphi_{5}}C_{\varphi_{6}}\\ S_{\varphi_{4}}S_{\varphi_{5}}&C_{\varphi_{4}}C_{\varphi_{6}}-S_{\varphi_{4}}C_ {\varphi_{5}}S_{\varphi_{6}}&-C_{\varphi_{4}}S_{\varphi_{6}}-S_{\varphi_{4}}C _{\varphi_{5}}C_{\varphi_{6}}\\ -C_{\varphi_{4}}S_{\varphi_{5}}&S_{\varphi_{4}}C_{\varphi_{6}}+C_{\varphi_{4}} C_{\varphi_{5}}S_{\varphi_{6}}&-S_{\varphi_{4}}S_{\varphi_{6}}+C_{\varphi_{4}} C_{\varphi_{5}}C_{\varphi_{6}}\end{bmatrix}  \tag{13}\]  
Here and below, the capital letters C and S mean cos and sin of the argument written as a subscript. On the other hand, the orientation of the gripper can be obtained from the conditions of the working process, when the initial and current angles \(\lambda_{E}\), \(\varphi_{E}\), \(\psi_{E}\) (or angles \(\lambda_{K}\), \(\varphi_{K}\), \(\psi_{K}\)) are known during the operational process, determined by the coordinates of three points of the working tool \(K\), \(D\), \(E\) (see Figure 2) according to Equations (1)-(4). To obtain the rotation matrix from the initial absolute CS \(OXYZ\) to the gripper CS _Kxyz_ through angles \(\lambda_{E}\), \(\varphi_{E}\), \(\psi_{E}\) (or angles \(\lambda_{K}\), \(\varphi_{K}\), \(\psi_{K}\)), it is necessary to evaluate the product of the matrices of elementary rotations in accordance with the sequence of such rotations (see Figure 2) for the angles \(\lambda_{E}\), \(\varphi_{E}\), \(\psi_{E}\), \(\
u\), \(\pi\) (for angles \(\lambda_{K}\), \(\varphi_{K}\), \(\psi_{K}\) in subsequent expressions, it is necessary to replace \(\lambda_{E}\), \(\varphi_{E}\), \(\psi_{E}\) with \(\lambda_{K}\), \(\varphi_{K}\), \(\psi_{K}\), and instead of \(\
u\)\(\xi=\arcsin\left(\frac{a}{c}\sin\
u\right)\) (see Figure 2) have to be used).