# Modeling and Compensation of Motion Errors for 6-DOF Robotic Manipulators
## 4 Simulation Studies for a 6-DOF Manipulator
### Solving Kinematics Model  
Inverse kinematics were utilized for the establishment of the joint angle compensation model, so this section solves the inverse kinematics for the 6-DOF manipulator. The DH parameters for the manipulator are listed in Table 1.  
According to the forward model in Section 2 and combined with the DH parameters of the 6-DOF manipulator, the following expressions can be obtained based on Equation (2):  
\[{}^{0}_{6}T=A_{1}A_{2}A_{3}A_{4}A_{5}A_{6}=\left[ \begin{array}{cccc}n_{x}&o_{ x}&a_{x}&p_{x}\\ n_{y}&o_{y}&a_{y}&p_{y}\\ n_{z}&o_{z}&a_{z}&p_{z}\\ 0&0&0&1\end{array} \right]\]  
where  
\[\left\{ \begin{array}{l}n_{x}=sin(\theta_{1}+\theta_{2})sin\theta_{6}+cos( \theta_{1}+\theta_{2})cos(\theta_{3}+\theta_{4}+\theta_{5})cos\theta_{6}\\ n_{y}=sin(\theta_{1}+\theta_{2})cos(\theta_{3}+\theta_{4}+\theta_{5})cos( \theta_{6})-sin\theta_{6}cos(\theta_{1}+\theta_{2})\\ n_{z}=sin(\theta_{3}+\theta_{4}+\theta_{5})cos\theta_{6}\\ \end{array} \right. \tag{33}\]  
\[\left\{ \begin{array}{l}o_{x}=-cos(\theta_{1}+\theta_{2})sin(\theta_{3}+ \theta_{4}+\theta_{5})\\ o_{y}=-sin(\theta_{1}+\theta_{2})sin(\theta_{3}+\theta_{4}+\theta_{5})\\ o_{z}=cos(\theta_{3}+\theta_{4}+\theta_{5})\\ \end{array} \right. \tag{34}\]  
\[\left\{ \begin{array}{l}a_{x}=sin(\theta_{1}+\theta_{2})cos\theta_{6}-cos( \theta_{1}+\theta_{2})cos(\theta_{3}+\theta_{4}+\theta_{5})sin\theta_{6}\\ a_{y}=-cos\theta_{6}cos(\theta_{1}+\theta_{2})-sin\theta_{6}sin(\theta_{1}+ \theta_{2})cos(\theta_{3}+\theta_{4}+\theta_{5})\\ a_{z}=-sin(\theta_{3}+\theta_{4}+\theta_{5})sin\theta_{6}\\ \end{array} \right. \tag{35}\]  
The inverse model for the manipulator can be solved by the following steps. To obtain \(\theta_{1},\theta_{5},\ \text{and}\ \theta_{6}\), transform the matrix of both sides of Equation (34):  
\[{}^{0}_{1}T^{-1}\cdot{}^{0}_{6}T\cdot{}^{5}_{6}T^{-1}={}^{1}_{5}T={}^{1}_{2}A ^{2}_{3}A^{3}_{4}A^{4}_{5}A \tag{36}\]  
The values in row 3 and column 4 are equal, and \(\theta_{1}\) is obtained:  
\[\theta_{1}=atan2(m_{1},n_{1})-atan2\bigg{(}d_{4},\pm\sqrt{{m_{1}}^{2}+{n_{1}} ^{2}-d_{4}^{2}}\bigg{)} \tag{37}\]  
where \(m_{1}=\big{(}p_{y}-d_{6}a_{y}\big{)}\) and \(n_{1}=(p_{x}-d_{6}a_{x})\). Using row 3, column 3 on both sides of the equation corresponds to the same thing, and then \(\theta_{5}\) is obtained:  
\[\theta_{5}=\pm\arccos\big{(}a_{y}c_{1}-a_{x}s_{1}\big{)} \tag{38}\] \begin{table}
\begin{tabular}{c c c c c} \hline \hline \multirow{2}{*}{**Joint Number**} & **Joint Angles** & **Connecting Link Length** & **Connecting Rod Torsion Angles** & **Connecting Rod Offset** \\  & \(\theta_{i}\) (\({}^{\circ}\)) & \(a_{i-1}\) (mm)** & \(\mathbf{a}_{i-1}\) (\({}^{\circ}\)) & \(\mathbf{d}_{i}\) (mm) \\ \hline
1 & \(\theta_{1}\) & 0 & \(\alpha_{1}=90\) & \(d_{1}=100.101\) \\
2 & \(\theta_{2}\) & \(a_{2}=-425.718\) & 0 & \(d_{2}=135.8\) \\
3 & \(\theta_{3}\) & \(a_{3}=-395.804\) & 0 & 0 \\
4 & \(\theta_{4}\) & 0 & \(\theta_{4}=-25.925\) \\
5 & \(\theta_{5}\) & 0 & \(a_{5}=90\) & \(d_{5}=97.612\) \\
6 & \(\theta_{6}\) & 0 & \(a_{6}=-90\) & \(d_{6}=82.613\) \\ \hline \hline \end{tabular}
\end{table} 
Table 1: DH parameters for the 6-DOF robotic manipulator. The third row and the first column on both sides of the equation are equal, and \(\theta_{6}\) is obtained:  
\[\theta_{6}=atan2(m_{6},n_{6})-atan2\Big{(}s_{5},\pm\sqrt{{m_{6}}^{2}+{n_{6}}^{2}- s_{5}^{2}}\Big{)} \tag{39}\]  
where \(m_{6}=\big{(}n_{x}s_{1}-n_{y}c_{1}\big{)}\)\(n_{6}=\big{(}o_{x}s_{1}-o_{y}c_{1}\big{)}\).