First draft at obstacle avoidance