## 2 The Manipulator Modelling  
The PUMA560 is a six-jointed robot with three joints used for determining the end effector's position, while the remaining three joints determine the wrist orientation. The axis of the first joint is vertical, while the axes of the second and third joints are horizontal and parallel, with a distance of a2 between them. The joint 1 and joint 2 axes intersect perpendicularly, while the axes of joints 3 and 4 intersect alternately at a distance of a3. The manipulator's schematic diagram is presented in Figure 1, while Figure 2 shows the corresponding link coordinate system, and Table 1 displays the corresponding modified D-H data [14].  
When solving for the manipulator with joint angles as variables, setting up a coordinate system for each joint of the manipulator is crucial. To construct the coordinate system for  
\begin{table}
\begin{tabular}{c c c c c c} \hline \hline
**Joint** & \(\mathbf{\theta_{i}}\)/(c) & \(\mathbf{\alpha_{i-1}}\)/(c) & \(\mathbf{\alpha_{i-1}}\)/mm & \(\mathbf{d_{i}}\)/mm & **Variable Range (c)** \\ \hline
1 & 90 & 0 & 0 & 0 & \(-\)160–160 \\
2 & 0 & \(-\)90 & 0 & 149.09 & \(-\)225–45 \\
3 & \(-\)90 & 0 & 431.8 & 0 & \(-\)45–225 \\
4 & 0 & \(-\)90 & 20.32 & 433.07 & \(-\)110–170 \\
5 & 0 & 90 & 0 & 0 & \(-\)100–100 \\
6 & 0 & \(-\)90 & 0 & 0 & \(-\)266–266 \\ \hline \hline \end{tabular}
\end{table}
Table 1: Modified PUMA560 D-H Parameters.  
Figure 1: PUMA560 schematic diagram.  
Figure 2: Coordinate system of PUMA560.  
each joint, the various parameters of the manipulator listed in Table 1 are needed. The range of rotation for each joint is primarily used to eliminate irrelevant solutions when the manipulator has multiple solutions during the solution process [15]. Each link can be transformed by its transformation matrix using the coordinate system shown in Figure 2 and the data provided in Table 1, as demonstrated in Equation (1):  
\[\underset{i}{}^{i-1}T=\begin{bmatrix}c\theta_{i}&-s\theta_{i}&0&a_{i-1}\\ s\theta_{i}\alpha_{i-1}&c\theta_{i}c\alpha_{i-1}&-s\alpha_{i-1}&-d_{i}s\alpha _{i-1}\\ s\theta_{i}s\alpha_{i-1}&c\theta_{i}s\alpha_{i-1}&c\alpha_{i-1}&d_{i}c\alpha_{ i-1}\\ 0&0&0&1\end{bmatrix} \tag{1}\]  
In Equation (1), \(c\theta\) represents \(\cos\theta\), and \(s\theta\) represents \(\sin\theta\). According to Equation (1), by substituting the data of PUMA560, the forward kinematics equation of PUMA560 can be obtained:  
\[\underset{6}{}^{0}T=\underset{1}{}^{0}T(\theta_{1})\underset{2}{}^{1}T( \theta_{2})\underset{3}{}^{2}T(\theta_{3})\underset{4}{}^{3}T(\theta_{4}) \underset{5}{}^{4}T(\theta_{5})\underset{6}{}^{5}T(\theta_{6}) \tag{2}\]  
Simplifying Equation (2) yields the forward kinematics equation for PUMA560:  
\[\underset{6}{}^{0}T=\begin{bmatrix}a_{x}&b_{x}&c_{x}&d_{x}\\ a_{y}&b_{y}&c_{y}&d_{y}\\ a_{z}&b_{z}&c_{z}&d_{z}\\ 0&0&0&1\end{bmatrix} \tag{3}\]  
The variables in the equation are defined as Equation (4):  
\[\left\{\begin{array}{l}a_{x}=c_{1}[c_{23}(c_{4}c_{5}c_{6}-s_{4}s_{6})-s_{23} s_{5}c_{6}]+s_{1}(s_{4}c_{5}c_{6}+c_{4}s_{6})\\ a_{y}=s_{1}[c_{23}(c_{4}c_{5}c_{6}-s_{4}s_{6})-s_{23}s_{5}c_{6}]-c_{1}(s_{4}c_{5 }c_{6}+c_{4}s_{6})\\ a_{z}=-s_{23}(c_{4}c_{5}c_{6}-s_{4}s_{6})-c_{23}s_{5}c_{6}\\ b_{x}=c_{1}[c_{23}(-c_{4}c_{5}c_{6}-s_{4}c_{6})+s_{23}s_{5}s_{6}]+s_{1}(c_{4}c_{ 6}-s_{4}c_{5}s_{6})\\ b_{y}=s_{1}[c_{23}(-c_{4}c_{5}s_{6}-s_{4}c_{6})+s_{23}s_{5}s_{6}]-c_{1}(c_{4}c_{ 6}-s_{4}c_{5}c_{6})\\ b_{z}=-s_{23}(-c_{4}c_{5}s_{6}-s_{4}c_{6})+c_{23}s_{5}s_{6}\\ c_{x}=-c_{1}(c_{23}c_{4}s_{5}+s_{23}c_{5})-s_{1}s_{4}s_{5}\\ c_{y}=-s_{1}(c_{23}c_{4}s_{5}+s_{23}c_{5})+c_{1}s_{4}s_{5}\\ c_{z}=s_{23}c_{4}s_{5}-c_{23}c_{5}\\ d_{x}=c_{1}[a_{2}c_{2}+a_{3}c_{23}-d_{4}s_{23}]-d_{2}s_{1}\\ d_{y}=s_{1}[a_{2}c_{2}+a_{3}c_{23}-d_{4}s_{23}]+d_{2}c_{1}\\ d_{z}=-a_{3}s_{23}-a_{2}s_{2}-d_{4}c_{23}\end{array}\right. \tag{4}\]  
Equation (4) describes the position and orientation of coordinate system [6] relative to the base coordinate system [0], where \(c_{23}=\cos(\theta_{2}+\theta_{3})\) and \(s_{23}=\sin(\theta_{2}+\theta_{3})\).