Added collision check to vertexes between the footprint points of a robot. Has to be tested