Add position locking
Revert "Add position locking"

This reverts commit 7353baca1df70a39a1e620c9dc28142323a760a1.

This is due to the fact that this code caused weird problems on
the robot.