Postprocessing Spline trajectories

[brianhou]

As @aditya-vk @gilwoolee @siddhss5 and I discussed via Slack, VectorFieldPlanner often produces extremely dense spline trajectories with many knots. We currently don't have a great way to postprocess these dense splines trajectories:

• Parabolic timer stops at every waypoint
• Parabolic smoother takes a very long time
• KunzRetimer incorrectly converts the knots of the spline into an interpolated trajectory before postprocessing them

These trajectories have mainly been used by planToEndEffectorOffset to either perform pushing/pulling/etc motions (post-processed with the incorrect KunzRetimer) or concatenated (by converting into interpolated trajectories and concatenating those).

Possible fixes:

• MinTOS (Implement a wrapper for MinTOS trajectory smoothing #213) should handle this correctly (and efficiently)
• Sparsify the trajectory before using parabolic smoother
• @siddhss5: Mike and I actually solved this for Robonaut by running an optimizer on the dense waypoints. You can fit a fixed order B-spline to the dense waypoints, check that it satisfies your workspace bounds and use that, or iterate.

I think the quickest and easiest temporary fix is to make explicit what we've been doing implicitly: return an interpolated trajectory from VectorFieldPlanner, until we have an efficient and correct way to postprocess dense splines. These other options sound like much better long-term solutions, but I think will also take more than the week we have before the next demo.

@mkoval we'd welcome any thoughts you have about this!