I am trying to initialize a (quintic spline) `Trajectory` using the functionality within ros_controllers/joint_trajectory_controller but am getting some undesired behavior when I sample the Trajectory constructed via `joint_trajectory_control::initJointTrajectory`.
In particular, when sampling times between the first (at time t=0.0) and second points, the spline returns always the second point's values (expected behavior for all times before the Trajectory's start time)
Additionally, when calling the initialization I get the following message:
> Dropping first 1 trajectory point(s) out of ### as they occur before the current time...
It seems that the initialization function is throwing away my first point (with `time_from_start = 0.0`) despite my trajectory starting from `ros::Time(0.0)` as specified in the passed message.
Digging through the code, the comments make me think that this should work but it is not. Is there perhaps a strict less-than compare instead of a less-than-or-equal or a missing check/case for `msg.header.time == 0.0`?
I seem to remember from somewhere that ros_control (in contrast to MoveIt!) assumes that the proved trajectory does not contain the current position and I assume that this is related to my issue BUT I feel that there is inherently something wrong with this approach. In particular, what about the case (mine currently) where the provided trajectory_msg for initialization is significantly sparse in time such that a significant number of sampled times are between the current pose and the first spline segment?
↧