## 3 Proposed Control Methodology
### Fault Detection-Based Virtual Energy Tank  
The conventional position-based impedance control is redesigned from [49] as  
\[\mathbf{D}_{m}\Delta\ddot{\mathbf{x}}+\mathbf{B}_{m}\Delta\dot{\mathbf{x}}+ \mathbf{K}_{m}\Delta\mathbf{x}=\mathbf{u}_{imp} \tag{30}\]  
where \(\Delta\mathbf{x}=\mathbf{x}_{d}-\mathbf{x}_{ref}\) and \(\mathbf{x}_{d},\mathbf{x}_{ref}\) denote the desired and reference trajectories. \(\mathbf{D}_{m},\mathbf{B}_{m},\mathbf{K}_{m}\) are matrices representing impedance behaviors. \(\mathbf{u}_{imp}\) is generated signal from force regulation and is defined as  
\[\mathbf{u}_{imp}=\mathbf{K}_{pf}(\mathbf{F}_{ext}-\mathbf{F}_{d})+\mathbf{K}_ {if}\int(\mathbf{F}_{ext}-\mathbf{F}_{d})+\mathbf{\eta}_{f}sign(\mathbf{F}_{ ext}-\mathbf{F}_{d}) \tag{31}\]  
with \(\mathbf{F}_{ext},\mathbf{F}_{d}\) denote vectors of external force induced from interaction with the environment and desired force to be tracked, respectively. \(\mathbf{K}_{pf},\mathbf{K}_{if}\) are proportional and integrated controller gains, and the second term \(\mathbf{\eta}_{f}sign(\mathbf{F}_{ext}-\mathbf{F}_{d})\) is added to increase the ability of tracking effort. In this section, an accidental contact-loss due to the step-change of environment is considered as a fault. Hence, a fault-detection-based virtual energy tank is suggested to decouple force regulation as prior safety criterion. \(\mathbf{x}_{t}\) is defined as a state associated with the tank that stores energy. The dynamics of the virtual energy tank variable is expressed by [34]:  
\[\dot{\mathbf{x}}_{t}=\beta\mathbf{x}_{t}^{-1}\Big{(}\Delta\mathbf{x}^{T} \mathbf{B}_{m}\Delta\dot{\mathbf{x}}-\gamma\Delta\dot{\mathbf{x}}^{T}\mathbf{ u}_{f}\Big{)}+\mathbf{u}_{t} \tag{32}\]  
where \(\mathbf{u}_{t}\) is the control input of the tank, and \(\mathbf{u}_{fi}\) is the additive control input signal of the force tracking impedance control scheme. \(\mathbf{u}_{t}\) and \(\mathbf{u}_{fi}\) are calculated by:  
\[\left( \begin{array}{c}\mathbf{u}_{fi}\\ \mathbf{u}_{t}\end{array} \right)=\left[ \begin{array}{cc}0&\kappa\\ -\kappa^{T}&0\end{array} \right]\left( \begin{array}{c}\Delta\mathbf{x}\\ \mathbf{x}_{t}\end{array} \right) \tag{33}\]  
with \(\kappa\) described as  
\[\kappa(\mathbf{F}_{ext},t)=\alpha(1-\gamma)\mathbf{x}_{t}^{-1}\mathbf{u}_{ imp} \tag{34}\]  
Three switching parameters \(\alpha,\beta,\gamma\) are defined as the following:  
\[\alpha=\left\{ \begin{array}{ll}1&\quad\text{if }T_{t}>T_{l}\\ 0&\quad\text{otherwise}\end{array} \right. \tag{35}\]  
\[\beta=\left\{ \begin{array}{ll}0&\quad\text{if }T_{t}\geq T^{u}\\ 1&\quad\text{otherwise}\end{array} \right. \tag{36}\]  
\[\gamma=\left\{ \begin{array}{ll}1&\quad\text{if }\Delta\dot{x}^{T}u_{imp} \leq 0\\ 0&\quad\text{otherwise}\end{array} \right. \tag{37}\]  
where \(T_{t}\) is energy stored in the virtual tank. \(\alpha\) is a switched parameter when the energy tank reaches its lower bound \(T_{l}\). \(\beta\) is a switched parameter in case the energy tank from the force/impedance controller exceeds an upper bound \(T^{u}\). Then, this parameter is used to detect an occurrence of contact-loss. \(\gamma\) is presented to guarantee the stability of the overall system. **Remark 9**. : _The value of the lower bound \(T_{l}\) is set to be greater than 0 to avoid the singularity.