Working obstacle avoidance, recursive avoidance not tested