Simulation of robot (or multiple robots) avoiding obstacles and each other to reach goal