Prevent the quad to fly outside the z-boundary of the map when planning in 3D

[fcladera]

Current status: when using a 3D planner, the quad might overhshoot along the z-direction when the stopping policy is called.

Ideal scenario: the quad is guaranteed to stay inside the map boundaries at all moments.