Algorithm 2. Strategy for obstacle presence determination and task recovery based on force feedback
Input: Real-time force feedback $f_i$; Collision threshold ${\delta }_m$; Original parameters $\{b_{md},k_{md}\}$; Output: Restored system parameters and trajectory;
1	if sustained contact force $\left|f_i\right|>{\delta }_m$ then
2		Maintain state: Output current avoidance parameters $b_{mi}$, $k_{mi}$ and maintain avoidance pose;
3	else (Obstacle removal determined: $\left|f_i\right|\le {\delta }_m$ and stable)
4		Smooth recovery: Guide variable parameters back to initial values $\{b_{md},k_{md}\}$ (Equations (6) and (7));
5		Trajectory reset: Output displacement deviation $e$ approaching 0 to return to desired trajectory $x_e\to 0$;
6	end if