## 2 Dynamic Modeling of 6R Robot System
### A simulation Example of the Dynamic Equation of the Constrained 6R Robot System  
There are 66 equations in the constraint dynamic equation of the robot system, which can solve up to 66 unknowns, usually corresponding to 36 generalized coordinates and 30 Lagrange multipliers. Since the generalized coordinates were measured in the simulation motion, 30 Lagrange multipliers can be solved as unknowns. Lagrange multipliers \(\lambda_{i}(i=1,2,\cdots,30)\) are written as a Lagrange multiplier matrix \(\lambda\underset{-i}{\lambda}(i=1,2,\cdots,6)\), which corresponds to rigid body \(B_{i}(i=1,2,\cdots,6)\). Each multiplier matrix is a five-order column matrix. For ease of description, it is recorded as:  
\[\underset{-i}{\lambda}=\left[ \begin{array}{cccc}\lambda_{i1}&\lambda_{i2}& \lambda_{i3}&\lambda_{i4}&\lambda_{i5}\end{array} \right]. \tag{112}\] \begin{table}
\begin{tabular}{c c c c c c c} \hline \hline \multirow{2}{*}{**Rigid Body**} & \multicolumn{6}{c}{**Absolute Error**} \\ \cline{2-7}  & \(\mathbf{m_{i}(e_{1}^{(0)})}\) & \(\mathbf{m_{i}(e_{2}^{(0)})}\) & \(\mathbf{m_{i}(e_{3}^{(0)})}\) & \(\mathbf{J_{i_{11}}^{(i)}}\) & \(\mathbf{J_{i_{22}}^{(i)}}\) & \(\mathbf{J_{i_{33}}^{(i)}}\) \\  & \(\mathbf{(kg)}\) & \(\mathbf{(kg)}\) & \(\mathbf{(kg)}\) & \(\mathbf{(kg\cdot m^{2})}\) & \(\mathbf{(kg\cdot m^{2})}\) & \(\mathbf{(kg\cdot m^{2})}\) \\ \hline \(B_{6}\) & \(-0.0001\) & \(0.0001\) & \(0\) & \(0.0002\) & \(0\) & \(0.0003\) \\ \hline \(B_{5}\) & \(-0.0002\) & \(-0.0002\) & \(0\) & \(-0.0002\) & \(-0.0005\) & \(0\) \\ \hline \(B_{4}\) & \(0.0005\) & \(0.0002\) & \(0.0002\) & \(0.0002\) & \(-0.0001\) & \(0.0006\) \\ \hline \(B_{3}\) & \(-0.0002\) & \(0\) & \(-0.0001\) & \(-0.0027\) & \(-0.0017\) & \(-0.0116\) \\ \hline \(B_{2}\) & \(-0.0006\) & \(-0.0003\) & \(-0.0003\) & \(0.0424\) & \(-0.0251\) & \(0.0175\) \\ \hline \(B_{1}\) & \(-0.0012\) & — & \(-0.0070\) & \(0.0001\) & \(-0.0006\) & \(-0.0006\) \\ \hline \end{tabular}
\end{table} 
Table 4: The absolute error between the dynamic parameters of rigid body \(B_{6},B_{5},\cdots,B_{1}\) and its real value. \(\lambda_{ij}(j=1,2,\cdots,5)\) denotes the five elements of B and each element has its physical meaning. Among them, \(\lambda_{i1},\lambda_{i2},\lambda_{i3}\) is proportional to the ideal constraint force of the rigid body in the \(\mathbf{e}_{1}^{(0)},\mathbf{e}_{2}^{(0)},\mathbf{e}_{3}^{(0)}\) direction and corresponds one to one. \(\lambda_{i4},\lambda_{i5}\) is the function of the three Cardan angles \(\alpha_{i},\beta_{i},\gamma_{i}\) of \(B_{i}\), which is proportional to the ideal constraint moment represented by the Cardano angle \(\alpha_{i},\beta_{i},\gamma_{i}\). The data at \(1.5\,\mathrm{s}\) time is taken, and the Lagrange multipliers are calculated. The result is calculated by Formula (111) as follows (the result retains seven significant digits):  
\[\lambda_{-6}=\left[ \begin{array}{ccccc}1.356146\times 10^{-4}&\vdots&3.870958 \times 10^{-4}&\vdots&-6.489101\times 10^{3}&\vdots&1.211238\times 10^{3}& \vdots&8.093237\times 10^{2}\end{array} \right]^{\mathrm{T}}. \tag{113}\]  
\[\lambda_{-5}=\left[ \begin{array}{ccccc}5.154353\times 10^{-4}&\vdots&-2.72 6663\times 10^{-4}&\vdots&-9.277012\times 10^{3}&\vdots&-1.631801\times 10^{10}& \vdots&-2.442163\times 10^{10}\end{array} \right]^{\mathrm{T}}. \tag{114}\]  
\[\lambda_{-4}=\left[ \begin{array}{ccccc}-3.836233\times 10^{-4}&\vdots&30.072190 \times 10^{-4}&\vdots&-1.230526\times 10^{4}&\vdots&8.285916\times 10^{3}& \vdots&5.536472\times 10^{3}\end{array} \right]^{\mathrm{T}}. \tag{115}\]  
\[\lambda_{-3}=\left[ \begin{array}{ccccc}2.941508\times 10^{-4}&\vdots&3.2 63078\times 10^{-4}&\vdots&2.393758\times 10^{4}&\vdots&-1.431100\times 10^{11}& \vdots&-2.141792\times 10^{11}\end{array} \right]^{\mathrm{T}}. \tag{116}\]  
\[\lambda_{-2}=\left[ \begin{array}{ccccc}1.797760\times 10^{-3}&\vdots&1.664961 \times 10^{-4}&\vdots&-5.086735\times 10^{4}&\vdots&-1.233380\times 10^{11}& \vdots&-1.845883\times 10^{11}\end{array} \right]^{\mathrm{T}}. \tag{117}\]  
\[\lambda_{-1}=\left[