TrajectoryOptimization Code for trajectory optimization intended for FRC robots. Currently creates a path based on cubic hermite splines. Does not (yet) do any actual optimization.