Use cubic spline for initial guess

[bruingineer]

goes along with SleipnirGroup/TrajoptLib#148

updates control interval counts based on trajoptlib calculated counts.

notes:

• straight-line constraint does not play nice with the spline trajectory initial guess.
• control intervals are calculated in trajoptlib based on trajectories between all waypoints (including initial guess waypoints). These counts are sent to Choreo, but when Choreo sends the counts back to trajoptlib, the intervals that include initial guesses have their counts summed together. This means for a path that has [pose, init guess, pose] waypoints, where trajoptlib gives intervals [12, 4, 1], Choreo will send the counts as [16, 1], which causes trajoptlib to split the 16 up evenly between initial guesses ( [ 8, 8, 1] ).