Implement smooth trajectories

[theochemel]

Pick a new type of smooth trajectories to use and write an implementation that matches our existing trajectory interface.
You can reference the min snap trajectories we have, but they take too long to calculate with long trajectories and are probably more complicated than we need.

Your implementation should accept:

• A list of (x, y, z) waypoints in the world NED frame
• Maximum velocity and acceleration constraints in XYZ and in yaw
• If possible, minimum and maximum z constraints to ensure we don't surface / hit the bottom of the pool

It should generate a trajectory that navigates through the set of waypoints as quickly as possible given the velocity and acceleration constraints, and orients the vehicle yaw forwards along the trajectory at all times.