Slip detection by keeping track of deviation from touchdown position

[LouKordos]

Maybe a good start for slip detection would be to store the touchdown position and define an area around that position that counts as minor slip, but outside that region the deviation is interpreted as slip.

Not sure how exactly to use this yet, maybe just overwrite gait phase to swing again and update planned contacts accordingly, at least let the MPC know in some way.