Quad forest

adds an example using the minDistanceConstraint to plan an obstacle-free path with the Quadrotor through a forest (most of the time :)
Do we need this assertion? What if I wan't to allow penetration up to a certain amount (e.g. if solving a sequence of optimization problems where the minimum distance is increased incrementally)?