fixed #1, trajectory is now checked for feasibility after smoothing operation