Trajectory planning library