multi_pi guides a quadorotor through a forest