Collision checking fixed (to the robot center). Must check for collision between two consecutive places
Collision checking for robot center fixed