## 2 Dynamic Modeling of 6R Robot System
### Constraint Equations for 6R Robotic Systems  
Each joint of the 6R robot is a single DOF revolute joint and each revolute joint provides five holonomic kinematic constraints. The revolute joints \(O_{j}\quad(j=1,2,\cdots,6)\) that links rigid body \(B_{j}\) to its inside connecting body \(B_{i(j)}\) has a base vector \(\mathbf{p}_{j}\) of shaft axis, as shown in Figure 7, the arrows represent the directions of the vectors and the circles represent the joint points in Figure 7.  
Then the kinematic constraints imposed by the joint \(O_{j}\) acting on the 6R robot system are embodied as follows:  
1. The joint point on rigid body \(B_{j}\) coincides with the joint point on its inside connecting body \(B_{i(j)}\) as a point. 2. The base vector of shaft axis of rigid body \(B_{j}\) coincides with the base vector of shaft axis of its inside connecting body \(B_{i(j)}\) as the same vector \(\mathbf{p}_{j}\). Constraint condition (1) is expressed as follows: \[\mathbf{r}_{i}+\mathbf{c}_{ij}-\mathbf{r}_{j}-\mathbf{c}_{jj}=0.\] (65)  
Figure 7: Constraints of revolute joint. Equation (65) is written in matrix form as follows:  
\[ \begin{array}{c}\underset{-i}{r}^{(0)}+\underset{-}{A}^{(0i)}\underset{-ij}{c}^ {(i)}-\underset{-j}{r}^{(0)}-\underset{-}{A}^{(0i)}\underset{-jj}{c}^{(j)}=0. \end{array}  \tag{66}\]  
Equation (66) projecting to \((O_{0},\underset{-}{e}^{(0)})\) produces three scalar equations, namely three constraints as follows:  
\[\phi_{j1}=x_{i}+\frac{l_{i}}{2}c\beta_{i}c\gamma_{i}-x_{j}+\frac{l_{j}}{2}c \beta_{j}c\gamma_{j}=0. \tag{67}\]  
\[\phi_{j2}=y_{i}+\frac{l_{j}}{2}(\alpha_{i}s\gamma_{i}+s\alpha_{i}s\beta_{i}c \gamma_{i})-y_{j}+\frac{l_{j}}{2}(\alpha_{j}s\gamma_{j}+s\alpha_{j}s\beta_{j}c \gamma_{j}). \tag{68}\]  
\[\phi_{j3}=z_{i}+\frac{l_{i}}{2}(s\alpha_{i}s\gamma_{i}-\alpha_{i}s\beta_{i}c \gamma_{i})-z_{j}+\frac{l_{j}}{2}(s\alpha_{j}s\gamma_{j}-c\alpha_{j}s\beta_{j}c \gamma_{j}). \tag{69}\]  
Constraint condition (2) is expressed as follows:  
\[\underset{-}{A}^{(0)}\underset{-}{p}_{j}^{(i)}-\underset{-}{A}^{(0)} \underset{-}{p}_{j}^{(j)}=0. \tag{70}\]  
The basis vector \(\boldsymbol{p}_{j}\) is projected in three directions to obtain three projection equations as follows:  
\[\phi_{j4}=p_{j1}^{(i)}c\beta_{i}c\gamma_{i}-p_{j2}^{(i)}c\beta_{i}s\gamma_{i} +p_{j3}^{(i)}s\beta_{i}-p_{j1}^{(j)}c\beta_{j}c\gamma_{j}+p_{j2}^{(j)}c\beta_{ j}s\gamma_{j}-p_{j3}^{(j)}s\beta_{j}=0. \tag{71}\]  
\[\phi_{j5}=p_{j1}^{(i)}(\alpha_{i}s\gamma_{i}+s\alpha_{i}s\beta_{i}c\gamma_{i}) +p_{j2}^{(i)}(\alpha_{i}c\gamma_{i}-s\alpha_{i}s\beta_{i}s\gamma_{i})-p_{j3}^ {(i)}s\alpha_{i}c\beta_{i} \tag{72}\]  
\[-p_{j1}^{(j)}(c\alpha_{j}s\gamma_{j}+s\alpha_{j}s\beta_{j}c\gamma_{j})-p_{j2}^ {(j)}(\alpha_{i}c\gamma_{j}-s\alpha_{j}s\beta_{j}s\gamma_{j})+p_{j3}^{(j)}s \alpha_{j}c\beta_{j}=0.\]  
\[\phi_{j6}=p_{j1}^{(i)}(s\alpha_{i}s\gamma_{i}-c\alpha_{i}s\beta_{i}c\gamma_{i} )+p_{j2}^{(i)}(s\alpha_{i}c\gamma_{i}+c\alpha_{i}s\beta_{i}s\gamma_{i})+p_{j3}^ {(i)}\alpha_{i}c\beta_{i}\]  
\[-p_{j1}^{(j)}(s\alpha_{j}s\gamma_{j}-c\alpha_{j}s\beta_{j}c\gamma_{j})-p_{j2}^ {(j)}(s\alpha_{j}c\gamma_{j}+c\alpha_{j}s\beta_{j}s\gamma_{j})-p_{j3}^{(j)} \alpha_{j}c\beta_{j}=0.\]  
Since the relationship between the projection components \(p_{j1},p_{j2},p_{j3}\) of \(\boldsymbol{p}_{j}\) is as follows:  
\[p_{j1}^{2}+p_{j2}^{2}+p_{j3}^{2}=1. \tag{74}\]  
Therefore, only two of the three projection formulas \(\phi_{j4},\phi_{j5},\phi_{j6}\) are independent. We only take \(\phi_{j4}\) and \(\phi_{j5}\) as the constraint equations.