# Dual PID Adaptive Variable Impedance Constant Force Control for Grinding Robot
## 4 6-Dof Force Sensor Gravity Compensation Strategy  
The gravity of the grinding tool generates components of force and moment in the sensor coordinate system. The force and moment \(F_{m}\) measured with the 6-Dof force sensor include: The grinding force \(F_{e}\) between the grinding tool and the workpiece, the gravity of the grinding tool \(F_{G}\), the inertial force \(F_{i}\) generated by the grinding tool because of the movement (since the feed speed of the grinding tool changes very little, so that \(F_{i}=0\)), and the sensor zero drift \(F_{0}\):  
\[{}^{S}F_{m}={}^{S}F_{e}+{}^{S}F_{G}+{}^{S}F_{i}+{}^{S}F_{0} \tag{35}\]  
where the superscript \({}^{S}\bullet\) indicates that the reference coordinate system of the force is in the sensor coordinate system \(\{\rm S\}\). Figure 7 shows the relationship between the world coordinate system \(\{\rm D\}\), the robot base coordinate system \(\{\rm B\}\), the end flange coordinate system \(\{\rm E\}\), the force sensor coordinate system \(\{\rm S\}\), and the grinding tool coordinate system \(\{\rm T\}\). The barycenter of the grinding tool is represented as \({}^{S}o(x,y,z)\) in the \(\{\rm S\}\) system. Using pins to maintain  
Selecting \(N\) different poses of the robot and using the least squares solution:  
\[A=\left({}^{\mathrm{S}}F_{m}^{\mathrm{T}}\cdot{}^{\mathrm{S}}F_{m}\right)^{-1} \cdot{}^{\mathrm{S}}F_{m}^{\mathrm{T}}\cdot{}^{\mathrm{S}}M_{m} \tag{40}\]  
The coordinate value \(x,y,z\) of tool barycenter and constant \(a_{1},a_{2},a_{3}\) in \(\{\mathrm{S}\}\) system is obtained. To obtain the component of the grinding tool in the \(\{\mathrm{B}\}\) system, it is necessary to consider the relationship between the \(\{\mathrm{B}\}\) system and the \(\{\mathrm{D}\}\) system. When the \(\{\mathrm{B}\}\) system rotates around the \(z\) axis of the \(\{\mathrm{D}\}\) system, there is no effect on the force sensor output. Consider that the \(\{\mathrm{B}\}\) system rotates around the \(x\)-axis and the \(y\)-axis of the \(\{\mathrm{D}\}\) system. Then, the rotation matrix \(\frac{D}{B}R\) between the \(\{\mathrm{B}\}\) system and the \(\{\mathrm{D}\}\) system is:  
\[ \begin{array}{c}{}_{B}^{D}R=\left[\begin{array}{ccc}1&0&0\\ 0&\cos\theta_{1}&-\sin\theta_{1}\\ 0&\sin\theta_{1}&\cos\theta_{1}\end{array}\right]\cdot\left[\begin{array}{ cccc}\cos\theta_{2}&0&\sin\theta_{2}\\ 0&1&0\\ -\sin\theta_{2}&0&\cos\theta_{2}\end{array}\right]\end{array}  \tag{41}\]  
where \(\theta_{1}\) and \(\theta_{2}\) are the angle around the \(x\)-axis and \(y\)-axis of the \(\{\mathrm{D}\}\) system, respectively. The component force of the grinding tool gravity \(G\) in the \(\{\mathrm{S}\}\) system can be represented by the robot homogeneous transformation matrix:  
\[{}^{\mathrm{S}}G=\left[ \begin{array}{c}{}^{\mathrm{S}}F_{\mathrm{G}x}\\ {}^{\mathrm{S}}F_{\mathrm{G}y}\\ {}^{\mathrm{S}}F_{\mathrm{G}z}\end{array} \right]=\begin{array}{c}{}_{B}^{ \mathrm{S}}R_{D}^{\mathrm{B}}R^{\mathrm{D}}G= \begin{array}{c}{}_{B}^{ \mathrm{S}}R\end{array} \left[ \begin{array}{c}{}_{G}\cos\theta_{1}\sin\theta_ {2}\\ -G\sin\theta_{1}\\ -G\cos\theta_{1}\cos\theta_{2}\end{array} \right] \tag{42}\]  
Let \(\left\{ \begin{array}{c}{}_{k}=G\cos\theta_{1}\sin\theta_{2}\\ {}_{b}=-G\sin\theta_{1}\\ {}_{b}=-G\cos\theta_{1}\cos\theta_{2}\end{array} \right..\)  
According to Equation (37) and Equation (42):  
\[\left[ \begin{array}{c}{}^{\mathrm{S}}F_{mx}\\ {}^{\mathrm{S}}F_{my}\\ {}^{\mathrm{S}}F_{mz}\end{array} \right]=\left[ \begin{array}{c}{}^{\mathrm{ S}}F_{\mathrm{G}x}\\ {}^{\mathrm{S}}F_{\mathrm{G}y}\\ {}^{\mathrm{S}}F_{\mathrm{G}z}\end{array} \right]+\left[ \begin{array}{c}{}_{ Fx0}\\ {}^{\mathrm{F}}_{y0}\\ {}^{\mathrm{S}}F_{\mathrm{G}z}\end{array} \right]=\begin{array}{c}{}_{B}^{ \mathrm{S}}R^{\mathrm{T}}\left[ \begin{array}{c}{}_{bx}\\ {}^{\mathrm{S}}U\\ {}^{\mathrm{S}}F_{\mathrm{G}z}\end{array} \right]+\left[ \begin{array}{c}{}_{ Fx0}\\ {}^{\mathrm{F}}_{y0}\\ {}^{\mathrm{S}}F_{z0}\end{array} \right] \tag{43}\]  
Rewriting Equation (43) in matrix form:  
\[\left[