Provide Derivatives in FunctionHandleTrajectory

[cohnt]

#21093 introduced a way for the user to specify trajectories as the image of an arbitrary function, completing the feature request in #20699. However, it does not yet support computing derivatives of the trajectory, which is needed for trajectory processing algorithms like Toppra. I think this should be pretty straightforward to implement, so I'll self-assign (and ask for help if I get stuck).

[cohnt]

Actually, maybe this is more complicated than I thought, since I don't think we can use AutoDiffXd to get higher order derivatives. Would love to hear if people have other ideas.

I know Russ suggested allowing a user to provide a function if the derivatives are known. Or we could provide a finite-difference approximation.

[cohnt]

@RussTedrake do you have any thoughts on how we should handle the user providing a function to give derivatives? I'm guessing the signature should be func(double t, int order), but Trajectory::DoEvalDerivative needs to work with all scalars, not just double. I'm not sure I even know what behavior would be expected if you call that method with an autodiff instead of double.

[RussTedrake]

the signature should be func(double t, int order)

That seems basically right to me. We should decide/recommend if it's ok for methods to throw if e.g. they don't support all positive orders.

Trajectory::DoEvalDerivative needs to work with all scalars, not just double.

That's true, and I think we're on the same page but just to make sure: you only need to support T=AutoDiffXd if people call set_derivative_callback (or whatever you call it) on a FunctionHandle<AutoDiffXd>. This is why the signature should actually be Matrix<T> func(const T& t, int order). And everything is well defined, even if T=AutoDiffXd (each scalar would have the derivatives of the derivative... that's fine). Symbolic Expression works, too.

[cohnt]

The interaction between std::function and templates is getting pretty far away from my comfort zone in C++. Can we maybe find someone else to get this done?

[rpoyner-tri]

Assigning to @hongkai-dai for further triage and delegation.