Make TrajoptLib kinematics use current and next variables instead of previous and current

[calcmogul]

The TrajoptLib kinematics constraints use previous and current variables: xₙ = xₙ₋₁ + vₙt + 1/2 aₙt².

      problem.SubjectTo(x_n_1 + v_n * dt_sgmt + a_n * 0.5 * dt_sgmt * dt_sgmt ==
                        x_n);

It's using the wrong velocity and acceleration for that timestep. It should be xₙ = xₙ₋₁ + vₙ₋₁t + 1/2 aₙ₋₁t² instead, and ideally xₙ₊₁ = xₙ + vₙt + 1/2 aₙt² to follow common convention.