Adding 3D obstacle avoidance