Prevent early collisions

[izvvv]

Why preventing premature collisions doesn't work ？

[eleurent]

I don't know, here the vehicle is overlapping with the goal location, but this case should to be handled by the np.linalg.norm(v.position - self.goal.position) < 5 case

Not sure why this is not working. Maybe you could add some print statements / breakpoints to display the vehicle-to-goal distance for each vehicle and see if we get one <5, why it doesn't get removed?