Stopped using sympy.geometry for dealing with points because it seems to be very sensitive to position errors; I'm using my own point class now and the robot leaves the obstacle following mode when appropriate