## 3 Robot Kinematic Equation Construction
### Constraint
#### 4.2.2 Tool Pose Constraint  
The change in the tool's posture and the position of the tool's tip will result in a change in the robot's posture. Therefore, in this section, the mapping relationship between the tool pose and the joint variables \(\theta_{j}\) of the robot is established, and the tool position is to limit the joint variables.  
The tool-tip of the ball-end milling cutter, i.e., the origin of the tool coordinate system \(([TCS])\), must strictly follow the machining curve during the machining process to ensure the accuracy of the milling trajectory. According to Equation (6), the tool pose in workpiece coordinate system \(([WCS])\) is obtained through homogeneous transformation.  
\[\begin{array}{l}w\\ {}_{tool}^{w}T(\theta)=\frac{w}{0}T_{6}^{0}T(\theta)_{tool}^{6}T=\left[\begin{array} []{cc}w&R(\theta)&w\\ 0&0&0\end{array}\right]P(\theta)\\ \frac{w}{0}T\text{ is the homogeneous coordinate transformation of }[WCS]\text{ with respect to }[RCS]\text{, and }\frac{6}{total}T\text{ is the homogeneous coordinate transformation of }[TCS]\text{ with respect to the end of the robot.}\\ \frac{w}{total}P(\theta)=\left[\begin{array}{cc}w\\ {}_{tool}^{w}P_{x}&{}_{tool}^{w}P_{y}&{}_{tool}^{w}P_{z}\end{array}\right]^{T} \text{ is the tool-tip path point in }[WCS]\text{ during the robot joint is in }\theta\text{. If the discretized curve points of the required tool path are \(P_{i}(x,\;y,\;z)\;(i=1\,\sim\,n)\), the robot end joint's position should be equal to the discretized curve points \(P_{i}(x,\;y,\;z)\;(i=1\,\sim\,n)\).  
\[\begin{array}{l}w\\ {}_{tool}^{w}P(\theta)=P_{i}(x,y,z)\end{array}\]During the milling process, the tool posture needs to be confined to a certain area. In this paper, the tool posture angle \((\alpha,\ \beta)\) of the tool axis with the local coordinate system \(([LCS])\) is selected to define the tool posture.  
According to Figure 3, taking the trajectory point as the origin, the normal vector of the machined surface as the \(Z\)-axis, and the tangent vector of the machined trajectory as the \(X\)-axis. \(\alpha\) is the angle between the tool axis and the \(Z\)-axis of \([LCS]\). \(\beta\) is the angle between the projection of the tool axis in the \(XOY\) plane of \([LCS]\) and the \(X\)-axis of \([LCS]\).  
The pose matrix \({}_{tool}^{l}T_{\theta}\) of \([WCS]\) with respect to \([LCS]\) is obtained by homogeneous transformation.  
\[{}_{tool}^{l}T(\theta)={}_{w}^{l}T_{tool}^{w}T(\theta)=\begin{bmatrix}{}_{tool }^{l}R(\theta)&{}_{tool}^{l}P(\theta)\\ 0\,0\,0&1\end{bmatrix} \tag{7}\]  
\({}_{w}^{l}T\) is the pose matrix of \([LCS]\) with respect to \([TCS]\).  
\({}_{tool}^{l}R(\theta)\) is the component of the \(Z\)-axis of \([TCS]\) in the three directions of \([LCS]\). The tool posture angle \((\alpha,\ \beta)\) can be calculated according to it:  
\[\alpha(\theta)=\cos^{-1}{}_{tool}^{l}a_{z} \tag{8}\]  
\[\beta(\theta)=\tan^{-1}{}_{tool}^{l}a_{z} \tag{9}\]  
Finally, the range of tool posture angle \((\alpha,\beta)\) is set as follows:  
\[\begin{array}{l}\alpha_{min}\leq\alpha(\theta)\leq\alpha_{max}\\ \beta_{min}\leq\beta(\theta)\leq\beta_{max}\end{array} \tag{10}\]