Collisions now detected more properly, still some issues with the robot