drake
drake copied to clipboard
RobotLocomotion
Represent the Image of a Trajectory Under Arbitrary Mappings
If I'm given a trajectory $q:[t_0,t_1]\to\mathbb R^n$, and a smooth function $f:\mathbb R^n\to\mathbb R^m$, then the composition $f\circ q:[t_0,t_1]\to\mathbb R^m$ is also a valid trajectory. However, there's no general way to represent such an object in Drake, so I can't use features such as Toppra (which requires a trajectory object as input).
My current workaround is to densely sample points from the trajectory and then interpolate them to construct a new trajectory, but this has been very finicky in practice (and is how I encountered the issue described in #20619). Such a feature would be extremely helpful for my constrained bimanual manipulation work, where robot trajectories live parametrization of configuration space that is more amenable to planning. Ideally, the feature would be compatible with functions defined with Drake's symbolic engine and "autodiff"-able functions.
cc @sageshoyu @shrutigarg914 @RussTedrake
Back in the matlab days of Drake, we had a FunctionHandleTrajectory class. We could bring that back easily enough. It would basically create a subclass of Trajectory that takes a std::function mapping time to the vector/matrix output. We can bind a python method to it via pybind. Would that be a sufficient solution?
Just to confirm my understanding of the workflow, I could write a (autodiff-compatible) function in Python, and use it to construct a FunctionHandleTrajectory? If I'm trying to remap a trajectory, I could just write a function that takes in a time t, computes the trajectory value at that time, and applies my desired transformation to that value? (Does autodiff play nicely throughout all that if I'm calling trajectory.value(t)?)
The Trajectory class is templated. The autodiff variant, Trajectory<AutoDiffXd>, when it calls value(t) would pass an autodiff and expect an autodiff output. And yes, we should be able to make that work for the python function, too. (We do this for mathematicalprogram costs and constraints).