## 3 Proposed Control Methodology
### Modification of Trajectory  
The previous section presented the effectiveness and stability of using the energy tank for contact-loss detection. However, this action only helps to decouple the signal command from force control from the overall controller scheme. Thereby, the end-effector follows the setup desired trajectory. Moreover, due to reaching the upper bound of the energy tank, the end-effector may attempt to regulate force until draining all energy in the virtual tank in some cases. Consequently, unsafe motions may be executed. To completely exhibit safety motion when contact-loss happens, a new trajectory should be designed to smooth the motion of the end-effector. For this purpose, a controlled shaping function \(\varphi(\zeta)\), where \(\zeta\) is a shift variable, is introduced as [36]  
\[\varphi_{i}=\left\{\begin{array}{ll}1&if\ F_{di}^{T}\psi_{i}\geq 0\\ \frac{1}{2}\Big{(}1+\cos\Big{(}\pi\frac{\zeta_{i}}{d_{\max}}\Big{)}\Big{)}&if \ \Big{(}F_{di}^{T}\psi_{i}<0\Big{)}\cap(0<\psi_{i}<d_{\max})\\ 0&\text{otherwise}\end{array}\right. \tag{48}\]  
\[\kappa_{\rho}=\varphi\kappa \tag{49}\]  
where \(\zeta_{i}=\Delta p_{i}-\Delta x_{si}sign(F_{di})\) (\(i=1,\,2,\,3\)) is a shifted variable, \(\Delta p\) is the error between the modified trajectory and actual position, \(\Delta x_{si}\) is an arbitrary constant that describes a robust region which denotes the distance between the end-effector to the set-point for shaping motion, and \(d_{\max}\) is an arbitrary constant that denotes the width of the region for shaping. It follows that if the desired force \(\mathbf{F}_{d}\) and the position error \(\Delta x\) encloses the robust region and passes the frontier, then the controller should be deactivated.  
The role of designing the shaping function is to handle unexpected motion when contact-loss happens. A shaping function \(\rho\) is introduced to generate a smooth transition between the force and impedance control; thus, providing a smooth transition in position performance of the end-effector.  
After generating a smooth transition between force and impedance control when contact loss occurs, the next important step is designing a suitable set-point such that the robot can follow without shock or unexpected motion. In order to avoid fast and unexpected motions in case of contact loss, which are caused by the impedance controller for large deflection from the set point, the set-point of the impedance control, called as commanded trajectory \(x_{c}\), is set close to the contact point \(\mathbf{x}_{contact}\) as \(\mathbf{x}_{c}=\mathbf{x}_{fade}=\mathbf{x}_{contact}-\epsilon_{set}\), where \(\mathbf{x}_{contact}=\mathbf{x}_{env}\) is the measured position of the end-effector at \(t_{0}\) and \(\epsilon_{set}\) is a small vector (indeed, when contacting with the environment, the measured position of the end-effector is the same as the environment. In the simulation, for illustration of impedance control, \(\mathbf{x}_{contact}\) is the actual position of the end-effector and this variable is different from the location of environment). A possible fading function is applied to obtain a continuous transition to the new set point. The flowchart for the fading trajectory is illustrated in Figure 4. From this illustration, the setpoint is determined based on the switching parameter \(\beta\) and comparison between the current and last position based time-delay than using predetermined or knowledge of the location of environment as in [35; 36].