initJointTrajectory in ros_controllers ignoring first JointTrajectoryPoint in provided message

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?

edit retag close merge delete