Potential Field Path Planning Python