## 2 Dynamic Modeling of 6R Robot System
### Graphical Description of the 6R Robot System
The direction of \(\boldsymbol{c}_{ij}\) is away from \(B_{0}\) and points to the outside. In particular, \(\boldsymbol{c}_{01}=\boldsymbol{C}_{01}=0\). The \(n\times n\) square matrix composed of \(\boldsymbol{c}_{ij}\) is called the matrix of the body-joint vector of the system, denoted as \(\underline{C}\). \(\underline{C}\) reflects the distribution of joints that has incidence with the rigid bodies of the system. For the 6R robot system,  
\[\boldsymbol{C}=\left[ \begin{array}{cccccc}\frac{l_{1}}{2}\boldsymbol{e}_{1}^ {(1)}&\frac{l_{1}}{2}\boldsymbol{e}_{1}^{(1)}&0&0&0&0\\ 0&\frac{l_{2}}{2}\boldsymbol{e}_{1}^{(2)}&\frac{l_{2}}{2}\boldsymbol{e}_{1}^{( 2)}&0&0&0\\ 0&0&\frac{l_{3}}{2}\boldsymbol{e}_{1}^{(3)}&\frac{l_{2}}{2}\boldsymbol{e}_{1}^ {(3)}&0&0\\ 0&0&0&\frac{l_{4}}{2}\boldsymbol{e}_{1}^{(4)}&\frac{l_{2}}{2}\boldsymbol{e}_{1}^ {(4)}&0\\ 0&0&0&0&\frac{l_{5}}{2}\boldsymbol{e}_{1}^{(5)}&\frac{l_{5}}{2}\boldsymbol{e}_{1 }^{(5)}\\ 0&0&0&0&0&\frac{l_{6}}{2}\boldsymbol{e}_{1}^{(6)}\end{array} \right]. \tag{11}\]  
The body-joint vector is shown in Figure 4.  
Third, the path matrix of force element for the 6R robot system is established and the resultant force of the force elements on the rigid body are derived. The force element is the internal force doing work in the system, noted as \(E_{k}\). The force elements in this paper are only the joint force elements, that is, the active forces of the inside joint and the outside joint on the rigid body. The \(k\)-th joint force element, \(E_{k}\), has incidence with a pair of rigid bodies, the inside connecting body of the hinge is also called the inside connecting body of the force element and is denoted as \(B_{e^{+}(k)}\). The outside connecting body of the joint is also called the outside connecting body of the force element and is denoted as \(B_{e^{-}(k)}\). The points of action of \(E_{k}\) on the outside connecting body and outside connecting body are noted as \(P_{k}^{+}\) and \(P_{k}^{-}\), respectively. As shown in Figure 6. The force of \(E_{k}\) on a body is denoted as \(\mathbf{F}_{k}^{\prime}\left(k=1,2,\cdots,n_{e}\right)\). The force \(\mathbf{F}_{k}^{\prime}\) of \(B_{e^{+}(k)}\) on \(B_{e^{-}(k)}\) points from \(P_{k}^{+}\) to \(P_{k}^{-}\) and the force \(-\mathbf{F}_{k}^{\prime}\) of \(B_{e^{-}(k)}\) on \(B_{e^{+}(k)}\) points from \(P_{k}^{-}\) to \(P_{k}^{+}\). Figure 4: The body-joint vector. Figure 5: The distribution of body-joint vectors of the 6R robot system. The vectors pointing from the centers of mass of rigid bodies \(B_{e^{+}(k)}\) and \(B_{e^{-}(k)}\) to the action points \(P_{k}^{+}\) and \(P_{k}^{-}\) of \(E_{k}\), respectively, are called force element vectors and are denoted as \(\mathbf{c}_{ik}^{e}\ \ (i=e^{+}(k)\ \ or\ \ e^{-}(k))\). The weighted force element vector is defined as:  
\[\mathbf{C}_{ik}^{e}=S_{ik}^{e}\mathbf{c}_{ik}^{e}\ \left( \begin{array}{c}i=1,2,\cdots,n \\ k=1,2,\cdots,n_{e}\end{array} \right). \tag{17}\]  
\(C_{ik}^{e}\) that has incidence with \(B_{i}\) is directed from the inside to the outside, and \(\mathbf{C}_{ik}^{e}\) that has no incidence with \(B_{i}\) is zero. The \(n\times n_{e}\) square matrix composed of \(\mathbf{C}_{ik}^{e}\) is called the vector matrix of force element of the system, denoted as \(\mathbf{C}^{e}\). \(\mathbf{C}^{e}\) is used to describe the distribution of force elements in each rigid body of the system. For the 6R robot system,  
\[\mathbf{C}^{e}=\left[ \begin{array}{ccccc}\frac{l_{1}}{2}\mathbf{e}_{1}^{(1)}&\frac{ l_{1}}{3}\mathbf{e}_{1}^{(1)}&0&0&0&0\\ 0&\frac{l_{2}}{2}\mathbf{e}_{1}^{(2)}&\frac{l_{2}}{2}\mathbf{e}_{1}^{(2)}&0&0&0\\ 0&0&\frac{l_{2}}{2}\mathbf{e}_{1}^{(3)}&\frac{l_{2}}{2}\mathbf{e}_{1}^{(3)}&0&0\\ 0&0&0&\frac{l_{2}}{2}\mathbf{e}_{1}^{(4)}&\frac{l_{2}}{2}\mathbf{e}_{1}^{(4)}&0\\ 0&0&0&0&\frac{l_{2}}{2}\mathbf{e}_{1}^{(5)}&\frac{l_{2}}{2}\mathbf{e}_{1}^{(5)}\\ 0&0&0&0&0&\frac{l_{2}}{2}\mathbf{e}_{1}^{(6)}\end{array} \right].