## 2 Dynamic Modeling of 6R Robot System
### Dynamical Modeling of Unconstrained 6R Robot System  
The vector of \(O_{ci}\) relative to \(O_{0}\) is denoted as \(\mathbf{r}_{i}\), the angular velocity of \(B_{i}\) relative to \((O_{0},\mathbf{e}_{-}^{(0)})\) is denoted as \(\omega_{i}\), the mass and central inertia tensor of \(B_{i}\) are denoted as \(m_{i}\) and \(J_{i}\), respectively, and each component \(\mathbf{e}_{1}^{(i)},\mathbf{e}_{2}^{(i)},\mathbf{e}_{3}^{(i)}\) of \((O_{i},\mathbf{e}_{-}^{(i)})\) is the central principal inertia axes of \(B_{i}\). Equating each rigid body to a homogeneous cylinder, the central inertia tensor of each rigid body is expressed as:  
\[\mathbf{J}_{-i}^{(i)}=\left[ \begin{array}{ccccc}J_{i_{11}}^{(i)}&0&0\\ 0&J_{i_{22}}^{(i)}&0\\ 0&0&J_{i_{33}}^{(i)}\end{array} \right]=\left[ \begin{array}{ccccc}\frac{1}{2} m_{i}R_{i}^{2}&0&0\\ 0&\frac{1}{12}m_{i}(3R_{i}^{2}+l_{i}^{2})&0\\ 0&0&\frac{1}{12}m_{i}(3R_{i}^{2}+l_{i}^{2})\end{array} \right]. \tag{21}\]The right-hand superscript \({}^{(i)}\) indicates an expression in \((O_{i},\underline{e}^{(i)})\) and no right-hand superscript is an expression in \((O_{0},\underline{e}^{(0)})\). \(J_{i_{11}}^{(i)}J_{i_{22}}^{(i)}J_{i_{33}}^{(i)}\) are the three principal moments of inertia. The Newton-Euler equations are derived by the principle of virtual power as follows:  
\[m_{i}\ddot{r}_{i}=F_{i}\ \ (i=1,2,\cdots,6). \tag{22}\]  
\[J_{i}\cdot\dot{\mathbf{\omega}}_{i}+\mathbf{\omega}_{i}\times(J_{i}\cdot\mathbf{\omega}_{i })=\mathbf{M}_{i}\ \ (i=1,2,\cdots,6). \tag{23}\]  
Here,  
\[\mathbf{F}_{i}=\mathbf{F}_{i}^{\delta}-\mathbf{F}_{i}^{\delta}=\mathbf{F}_{i}^{\delta}-\sum \limits_{k=1}^{n_{c}}S_{ik}^{\epsilon}\mathbf{F}_{k}^{\epsilon}. \tag{24}\]  
\[\mathbf{M}_{i}=\mathbf{M}_{i}^{\delta}+\mathbf{M}_{i}^{\delta}=-\sum\limits_{k=1}^{n_{c}} \left(\mathbf{C}_{ik}^{\epsilon}\times\mathbf{F}_{k}^{\epsilon}+\mathbf{S}_{ik}\times\mathbf{ M}_{k}^{\epsilon}\right). \tag{25}\]  
\(\mathbf{F}_{i}^{\delta}\) is the gravity of \(B_{i}\), \(\mathbf{M}_{i}^{\delta}\) is the active moment of \(O_{i}\) on \(B_{i}\), \(\mathbf{M}_{i}^{\delta}=-\mathbf{S}_{ik}\times\mathbf{M}_{k}^{\delta}\mathbf{M}_{i}^{\delta}=- \sum\limits_{k=1}^{n_{c}}\mathbf{S}_{ik}\times\mathbf{M}_{k}^{\delta}\) is the active resultant moment of the joints that has incidence with \(B_{i}\) on \(B_{i}\). For the 6R robot system,  
\[ \begin{array}{c}\mathbf{M}_{-}^{\delta}=-\mathbf{S}\times\mathbf{M}_{-}^{\delta}=\left[ \begin{array}{c}\mathbf{M}_{1}^{\delta}-\mathbf{M}_{2}^{\delta}\\ \mathbf{M}_{2}^{\delta}-\mathbf{M}_{3}^{\delta}\\ \mathbf{M}_{3}^{\delta}-\mathbf{M}_{4}^{\delta}\\ \mathbf{M}_{4}^{\delta}-\mathbf{M}_{5}^{\delta}\\ \mathbf{M}_{5}^{\delta}-\mathbf{M}_{6}^{\delta}\end{array}\right].\end{array}  \tag{26}\]  
\(\mathbf{M}_{-}^{\delta}=\left[ \begin{array}{cc}\mathbf{M}_{1}^{\delta}&\mathbf{M}_{2}^{ \delta}&\mathbf{M}_{3}^{\delta}&\mathbf{M}_{4}^{\delta}&\mathbf{M}_{5}^{\delta}&\mathbf{M}_{6}^ {\delta}\end{array} \right]^{\mathrm{T}}\) is the column matrix composed of \(\mathbf{M}_{i}^{\delta}\ \ (i=1,2,\cdots,6)\). Therefore, for the 6R robotics system,  
\[ \begin{array}{c}\mathbf{M}=\mathbf{M}^{\delta}+\mathbf{M}^{\delta}=\left[\begin{array}[ ]{cc}(F_{1}^{\epsilon}+F_{2}^{\epsilon})\times\frac{l_{1}}{2}\mathbf{e}_{1}^{(1)} \\ (F_{2}^{\epsilon}+F_{3}^{\epsilon})\times\frac{l_{2}}{2}\mathbf{e}_{1}^{(1)}\\ (F_{3}^{\epsilon}+F_{4}^{\epsilon})\times\frac{l_{2}}{2}\mathbf{e}_{1}^{(3)}\\ (F_{4}^{\epsilon}+F_{5}^{\epsilon})\times\frac{l_{2}}{2}\mathbf{e}_{1}^{(4)}\\ (F_{5}^{\epsilon}+F_{6}^{\epsilon})\times\frac{l_{2}}{2}\mathbf{e}_{1}^{(5)}\\ (F_{6}^{\epsilon})\times\frac{l_{6}}{2}\mathbf{e}_{1}^{(6)}\end{array}\right]+ \left[\begin{array}{c}\mathbf{M}_{1}^{\delta}-\mathbf{M}_{2}^{\delta}\\ \mathbf{M}_{2}^{\delta}-\mathbf{M}_{3}^{\delta}\\ \mathbf{M}_{3}^{\delta}-\mathbf{M}_{4}^{\delta}\\ \mathbf{M}_{4}^{\delta}-\mathbf{M}_{5}^{\delta}\\ \mathbf{M}_{6}^{\delta}\end{array}\right].\end{array}  \tag{27}\]  
\(\mathbf{M}=\left[