Work on collision detection in robotTrajectory2d