# Research on the Dynamic Control Method of CFETR Multi-Purpose Overload Robot
## 2 CMOR Modelling
### CMOR Dynamics Model
CMOR potential energy is mainly the link gravitational potential energy \(P_{gl}\), joint rotor gravitational potential energy \(P_{gr}\), and end-effector and load gravitational potential energy \(P_{gp}\)  
\[P=P_{gl}+P_{gr}+P_{gp}+\Delta P \tag{12}\]  
where \(\Delta P\) is the potential energy calculation error. The gravitational potential energy at position \(r^{i}\) on any link \(i\) of the CMOR is  
\[\frac{0}{8}T=\frac{0}{1}TR_{22}^{1}T_{3}^{2}T\cdots \frac{6}{7}T_{8}^{7}T \tag{13}\]  
where \(g^{T}=\left[ \begin{array}{cccc}0&0&9.81&1\end{array} \right]^{T}\). Integrating Equation (13), the total potential energy of each link of CMOR is obtained as  
\[P_{gl}=\sum_{i=1}^{8}-g^{T}T_{i}\int_{link_{i}}r^{i}dm=-\sum_{i=1}^{8}m_{gi}g^ {T}T_{i}r_{i}^{i} \tag{14}\]  
where \(m_{gi}\) is the mass of joint \(i\) and \(r_{i}^{i}\) is the centre of gravity of joint \(i\) concerning the front joint coordinate system. CMOR rotor gravitational potential energy of any joint \(i\)  
\[P_{gr}=\sum_{i=1}^{8}-m_{ri}g^{T}T_{i}r_{r}^{i} \tag{15}\]where \(m_{ri}\) is the joint \(i\) rotor mass and \(r_{r}^{i}\) denotes the rotor centre of mass concerning the joint \(i\) coordinates. The gravitational potential energy of the loaded mass of the CMOR end-effector is  
\[P_{gp}=-m_{gp}g^{T}T_{8}r_{S}^{8} \tag{16}\]  
where \(m_{gp}\) is the total mass of the end-effector and the load, and \(r_{S}^{8}\) represents the coordinates of the load position relative to the end joint. The third step derives the CMOR dynamic equations. CMOR is a typical serial multi-joint robotic arm and the final dynamics model can be expressed as  
\[T=M(q)\ddot{q}+H\big{(}q,\dot{q}\big{)}\dot{q}+G(q)+D \tag{17}\]  
where \(\dot{q}\in R^{8}\) is the acceleration vector, \(\dot{q}\in R^{8}\) is the velocity vector, \(T\in R^{8}\) is the torque vector, \(M(q)\in R^{8\times 8}\) is the inertial force matrix, \(H(q,\dot{q})\dot{q}\in R^{8\times 8}\) is the centrifugal and Gauchy force matrix, \(G(q)\in R^{8}\) is the gravity vector, and \(D\) denotes the bounded unknown perturbation. The torque of any joint \(p\) in Equation (17) is calculated using the Lagrangian equation as  
\[ \begin{array}{l}T_{p}=\frac{d}{dt}\frac{\partial L}{\partial q_{p}}-\frac{ \partial L}{\partial q_{p}}=N_{p}^{2}I_{a}\ddot{q}_{p}+\sum_{i=p}^{8}\sum_{k=1 }^{i}Trace\bigg{(}\frac{\partial T_{i}}{\partial q_{k}}I_{i}\frac{\partial T_ {i}^{T}}{\partial q_{p}}\bigg{)}\ddot{q}_{k}\\ \quad+\sum_{i=p}^{8}\sum_{j=1}^{i}\sum_{k=1}^{i}Trace\bigg{(}\frac{\partial^{2 }T_{i}}{\partial q_{j}\partial q_{k}}I_{i}\frac{\partial T_{i}^{T}}{\partial q _{p}}\bigg{)}\dot{q}_{j}\dot{q}_{k}+m_{p}\sum_{j=p}^{8}Trace\bigg{[}\frac{ \partial T_{8}}{\partial q_{j}}r_{p}^{8}\\ \quad\Big{(}r_{p}^{8}\Big{)}^{T}\Big{(}\frac{\partial T_{8}}{\partial q_{k}} \Big{)}\bigg{]}\ddot{q}_{j}+m_{p}\sum_{j=p}^{8}\sum_{k=p}^{8}Trace\bigg{(} \frac{\partial T_{8}}{\partial q_{j}\partial q_{k}}r_{p}^{8}\Big{(}r_{p}^{8} \Big{)}^{T}\Big{(}\frac{\partial T_{8}}{\partial q_{p}}\Big{)}\bigg{]}\dot{q }_{j}\dot{q}_{k}\\ \quad-\sum_{i=p}^{8}m_{i}^{8}g^{T}\frac{\partial T_{i}}{\partial q_{p}}r_{pk}^ {i}-\sum_{i=p}^{8}m_{i}^{r}g^{T}\frac{\partial T_{i}}{\partial q_{p}}r_{r}^{ i}-m_{p}^{8}g^{T}\frac{\partial T_{8}}{\partial q_{p}}r_{S}^{8}\\ \quad-K_{p}\Big{(}q_{p}^{l}-q_{p}\Big{)}\end{array}  \tag{18}\]  
where the Lagrangian function \(L\) is  
\[ \begin{array}{l}L=K-P\\ =\frac{1}{2}\sum_{i=1}^{8}N_{i}^{2}\dot{q}_{i}^{2}I_{ai}+\frac{1}{2}\sum_{i=1}^ {8}\sum_{j=1}^{i}\sum_{k=1}^{i}Trace(\frac{\partial T_{i}}{\partial q_{j}}I_{i }\frac{\partial T_{i}^{T}}{\partial q_{k}})\dot{q}_{j}\dot{q}_{k}\\ \quad+\frac{1}{2}m_{p}Trace\bigg{[}\sum_{j=1}^{8}\sum_{k=1}^{8}\frac{\partial T _{k}}{\partial q_{j}}r_{k}(r_{S})^{T}\Big{(}\frac{\partial T_{k}}{\partial q_{ k}}\Big{)}\dot{q}_{j}\dot{q}_{k}\bigg{]}+\frac{1}{2}\sum_{i=1}^{8}K_{i}(q_{i}^{l}-q_{i})^{2} \\ \quad+\sum_{i=1}^{8}m_{gi}g^{T}T_{i}r_{i}^{i}+\sum_{i=1}^{8}m_{ri}g^{T}T_{i}r_{ r}^{i}+m_{g}^{8}g^{T}T_{8}r_{S}^{8}\end{array}  \tag{19}\]