Algorithm 1.

Contact Detection

1: procedure ContactDetection $\left({\mathit{\psi }}_{cntrl},{\mathit{\psi }}_e,{\mathbf{C}}_b\right)$

2:  ${\mathbf{w}}^b\leftarrow {\stackrel{-}{\mathbf{w}}}_b$

3:  repeat

4:   ${\mathbf{w}}^b\leftarrow {\mathbf{w}}^b+{\mathbf{C}}_b^{†}{\mathit{\psi }}_{cntr}$

5:   if $∥{\mathit{\psi }}_e∥>{\mathit{\zeta }}_{\psi }$ then

6:    Possible Contact ← true

7:    ${\mathbf{w}}_c={\mathbf{w}}^b$

8:   else

9:    Possible Contact ← false

10:    ${\mathbf{w}}_c=\mathbf{0}$

11:   end if

12:   if Possible Contact == true then

13:    if $∥{\left.{\mathbf{w}}^b\right|}_x-{\left.{\mathbf{w}}_c\right|}_x∥>{\mathit{\zeta }}_{\sigma }$ then

14:     if ${\left.{\mathit{\psi }}_{cntrl}\right|}_x>0$ then

15:      Contact Triggered X+ ← true

16:     else

17:      Contact Triggered X- ← true

18:     end if

19:    end if

20:    if $∥{\left.{\mathbf{w}}^b\right|}_y-{\left.{\mathbf{w}}_c\right|}_y∥>{\mathit{\zeta }}_{\sigma }$ then

21:     if ${\left.{\mathit{\psi }}_{cntr}\right|}_y>0$ then

22:      Contact Triggered Y+ ← true

23:     else

24:      Contact Triggered Y- ← true

25:     end if

26:    end if

27:   end if

28:  until Reset == true

29:  return true

30: end procedure