Plan dynamically feasible trajectories

[nicovanduijn]

This is on top of #448 and #443 so those should go in first.

This uses the trajectory simulator in the tree generation to add nodes to the tree that we can actually reach.

Note that this does not yet affect the selection of the nodes, whose cost is still computed by assuming perfect tracking.

Also the eventual setpoint will be on the dynamically feasible path, not the setpoint input to the simulator used to calculate the node position. This sounds like flawed logic, but it yields very smooth and nice flying behavior in SITL.