stuck function

We are now fixing the weird case of getting stuck somewhere with
consecutive rotation pi/2 -pi/2 -pi/2 pi/2 or -pi/2 pi/2 -pi/2 pi/2