# Motion Control and External Force Estimation of a Pneumatically Driven Multi-DOF Robotic Forceps
## 3 Control Scheme
### External Force Estimation  
We estimated the external force exerted on the tip of the forceps by using the disturbance observer control scheme.  
If there is no external force, the driving torque of the joints is equal to their dynamics. The external force exerted on the tip of the forceps works as a torque (\(\tau_{\varphi\_\_\text{ext}}\)) to each mechanical joint expressed by the mechanical joint angles \(\varphi\). The relationship between \(\tau_{\varphi\_\text{ext}}\) and the driving torque of the mechanical joint \(\tau_{\varphi}\) is expressed by the following function:  
\[\tau_{\varphi}-\mathbf{Z}=\tau_{\varphi\_\text{ext}} \tag{5}\]  
The forceps motion angle \(\mathbf{q}\) is more intuitive to represent the motion of the whole forceps. Hence, it is more appropriate for calculating the external force to use the converted torque parameter \(\tau_{q}\) about the forceps motion angle \(\mathbf{q}\). By using the principle of virtual work on (1), \(\tau_{q}\) can be calculated by the following function:  
\[\tau_{q}=\left(\mathbf{J}_{\text{link}}^{T}\right)^{-1}\tau_{\varphi}=\left[ \begin{array}{ccc}1&0&0\\ 0&1&1\\ 0&1/2&-1/2\end{array}\right]\tau_{\varphi} \tag{6}\]  
We herein denote \(\tau_{q\_\text{ext}}=\left(\tau_{q\_\text{ext}},\ \tau_{q\_\text{2,ext}},\ \tau_{q\_\text{3,ext}}\right)^{T}\) as the external torque about the motion angle \(\mathbf{q}\). Then, \(\tau_{q\_\text{ext}}\) can be calculated from \(\tau_{\varphi\_\text{ext}}\) by using the same relationship expressed by (6).  
\[\tau_{q\_\text{ext}}=\left(\mathbf{J}_{\text{link}}^{T}\right)^{-1}\tau_{\varphi \_\text{ext}} \tag{7}\]  
In this work, we separately estimate the external translational force \(\mathbf{f}_{\text{ext}}\) on the forceps tip and the external grasping force \(f_{\text{ext\_g}}\) on the gripper. The three components of \(\mathbf{f}_{\text{ext}}\) in the orthogonal coordinate system presented in Figure 4 are calculated from the two joint torque components in \(\tau_{q\_\text{ext}}\) by using the Jacobian matrix of a 2-DOF serial link mechanism as follows.  
\[\mathbf{f}_{\text{ext}}=\left[\begin{array}{c}f_{\text{ext\_x}}\\ f_{\text{ext\_y}}\\ f_{\text{ext\_z}}\end{array}\right]=\left(\mathbf{J}_{a}^{T}\right)^{+}\tau_{q12\_ \text{ext}}=\left[\begin{array}{ccc}0&\frac{1}{\left(l_{1}+l_{2}\right)\cos q _{1}}\\ \frac{-1}{l_{2}\cos q_{2}}&0\\ \frac{-1}{\left(l_{1}+l_{2}\cos q_{2}\right)\sin q_{1}}&\frac{-1}{l_{2}\sin q _{2}}\end{array}\right]\left[\begin{array}{c}\tau_{q1\_\text{ext}}\\ \tau_{q2\_\text{ext}}\end{array}\right] \tag{8}\]  
\(\mathbf{J}_{a}\) is the Jacobian matrix of a 2-DOF serial link manipulator, which translates the angular velocity of the two serial link joints to the translational velocity of the end effector in Cartesian coordinates. \(\tau_{q12\_\text{ext}}=\left(\tau_{q1\_\text{ext}},\ \tau_{q2\_\text{ext}} \right)^{T}\) is a 2-DOF vector consisting of the first two elements of \(\tau_{q\_\text{ext}}\).  
\begin{table}
\begin{tabular}{c c c} \hline \hline
**Model Parameter** & \(\mathbf{i=1}\) & \(\mathbf{i=gL}\), \(\mathbf{gR}\) \\ \hline \(C_{i}\) (mNm-s/rad) & 1.0 & 0.05 \\ \(D_{i}\) (mNm) & 9.0 & 5.0 \\ \(\mu\) & - & 0.25 \\ \hline \hline \end{tabular}
\end{table}
Table 1: Dynamic model parameters used in the control system.  
\(l_{1}\) denotes the length of the first joint and \(l_{2}\) denotes the length from the tip at the gripper finger to its rotation center as presented in Figure 2.  
Regarding the grasping force, we used the third element of \(\tau_{q\_\text{ext}}\), which can be calculated as follows:  
\[f_{\text{ext\_g}}=\frac{1}{l_{2}}\tau_{q3\_\text{ext}} \tag{9}\]