added Trajectory::Swap, several bug fixes with constraintparabolicsmoother, added planningutils::InsertWaypointWithRetiming
fixed bug in trajectory timing parameters