# Dynamic Modeling and Response Analysis of an Optical Machining Robot with Prismatic Joint Clearance
## 3 Dynamic Modeling of 3-UPRU+UP with Prismatic Joint Clearance
### Dynamic Modeling with Joint Clanance  
The constraint equation at the clearance is substituted with the contact force constraint, and the constraint equation corresponding to Equation (14) is removed. The constraint equation with prismatic joint clearance is derived as follows:  
\[\mathbf{\Phi}_{\text{s}}(q)=\begin{bmatrix}\mathbf{\Phi}_{U11}&\mathbf{\Phi}_{U2}&\mathbf{\Phi}_ {U3}&\mathbf{\Phi}_{U5}&\mathbf{\[PERSON]}_{U16}&\mathbf{\[PERSON]}_{U7}&\mathbf{\[PERSON]}_{U}\\ \mathbf{\Phi}_{P1}&\mathbf{\Phi}_{P2}&\mathbf{\Phi}_{P3}&\mathbf{\Phi}_{R1}&\mathbf{\Phi}_{R2}&\bm {\Phi}_{R3}&\mathbf{\Phi}_{D}\end{bmatrix}^{\text{T}}=\mathbf{0}_{60\times 1} \tag{15}\]  
Through the calculation of partial derivatives of Equation (14) in terms of \(t\) (time), the velocity and acceleration constraint equations of the system are solved as follows:  
\[\mathbf{\[PERSON]}_{sq}\dot{\mathbf{q}}=-\mathbf{\Phi}_{st}=\mathbf{v} \tag{16}\]  
\[\mathbf{Q}_{sq}\ddot{\mathbf{q}}=-\Big{(}\mathbf{Q}_{sq}\dot{\mathbf{q}}\Big{)}_{q}\dot{\mathbf{q} }-2\mathbf{\Phi}_{sq}\dot{\mathbf{q}}-\mathbf{\Phi}_{stt}=\mathbf{\gamma} \tag{17}\]  
where \(\mathbf{\Phi}_{sq}\) is the matrix of derivatives of the constraint equations in terms of the generalized coordinates \(\mathbf{q}\); \(\dot{\mathbf{q}}\) represents the first derivative of the generalized coordinate; \(\mathbf{\Phi}_{st}\) is the derivative of the constraint equation; \(\mathbf{v}\) is the right side of the velocity constraint equation; \(\ddot{\mathbf{q}}\) is the generalized acceleration column vector; \(\mathbf{\Phi}_{sq}\) is the derivative matrix of the Jacobian matrix of the constraint equation; and \(\mathbf{\gamma}\) is the right side of the acceleration constraint equation.  
The dynamic equation of the 3-UPRU+UP parallel manipulator with Lagrange multipliers is obtained as follows:  
\[\mathbf{M}\ddot{\mathbf{q}}+\mathbf{\Phi}_{sq}^{\text{T}}\lambda=\mathbf{F} \tag{18}\]  
where \(\mathbf{M}\) is the generalized mass matrix; \(\lambda\) is the Lagrange multiplier; \(\mathbf{\Phi}_{sq}^{\text{T}}\lambda\) is related to the joint constraint reaction force/torque and driving force/torque; and \(\mathbf{F}\) is the generalized external forces.  
By combining Equations (17) and (18), we can obtain the following dynamic equation:  
\[\begin{pmatrix}\mathbf{M}&\mathbf{\Phi}_{sq}^{\text{T}}\\ \mathbf{\Phi}_{sq}&0\end{pmatrix}\begin{pmatrix}\ddot{\mathbf{q}}\\ \lambda\end{pmatrix}=\begin{pmatrix}\mathbf{F}\\ \mathbf{\gamma}\end{pmatrix} \tag{19}\]  
When solving dynamic equations using integral methods, the accumulation of errors over time leads to growing constraint violations, which reduces the accuracy of the final results and may even produce incorrect outcomes. To improve this method, [PERSON] (1989) applied the correction idea from control systems, making corrections in real time based on the violation of position and velocity constraints. Equation (19) can be rewritten as:  
\[\begin{bmatrix}\mathbf{M}&\mathbf{\Phi}_{sq}^{\text{T}}\\ \mathbf{\Phi}_{sq}&\mathbf{0}\end{bmatrix}\begin{bmatrix}\ddot{\mathbf{q}}\\ \lambda\end{bmatrix}=\begin{bmatrix}\mathbf{F}\\ \mathbf{\gamma}\text{-}2\alpha_{b}\dot{\mathbf{\Phi}}_{s}\text{-}\beta_{b}^{2}\mathbf{ \Phi}_{s}\end{bmatrix} \tag{20}\]  
where \(\alpha_{b}\) and \(\beta_{b}\) are stability coefficients, and their values are selected from experience. \(2\alpha_{b}\dot{\mathbf{\Phi}}_{s}\) and \(\beta_{b}^{2}\mathbf{\Phi}_{s}\) play the role of control terms. The function ode45 of the MATLAB method was used to integrate the differential equation of the 3-UPRU+UP parallel manipulator. When the Baumgarte parameters are taken \(\alpha_{b}\) = 5 and \(\beta_{b}\) = 1, the violation amount of the position constraint equation is below 1 \(\times\) 10\({}^{-10}\) m.  
The dynamic simulation solution flowchart of the 3-UPRU+UP parallel manipulator considering the clearance joint is shown in Figure 7.