Force recalculation of jerk when necessary, and fixed some velocity selections.
Testing replanting if we change entry_velocity.
Testing. DO NOT USE.
More testing
More testing.
Partially implemented ne Forward Differencing and trapezoid calc.
“Pop” forward differencing working. (STILL TESTING. DO NOT USE.)