added quadric, quintic, and sextic interpolations to trajectory
fix quintic interpolation
minor traj interpolation