[JTC] When replacing trajectories in open-loop mode use time of the last command when setting point of trajectory before msg.

[destogl]

This PR tackles the issues that happen in open-loop mode where on replacing trajectories the last command is repeated.
The figures below show the results of the output trajectories before and after this functionality. The trajectory that uses the time of the last command as point before trajectory messages gets smoother output.

Functionality right now:

Functionality with this PR:

P.S. The similar thing would be useful for trajectory replacement in closed loop mode, but then we would need to know exactly when the state is read from the hardware. This is just something to think about in the future.

P.P.S. I will add tests too.

[codecov]

Codecov Report

Merging #780 (8fc35dc) into master (232154d) will decrease coverage by 0.07%.
The diff coverage is 0.00%.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #780      +/-   ##
==========================================
- Coverage   45.40%   45.34%   -0.07%     
==========================================
  Files          40       40              
  Lines        3649     3654       +5     
  Branches     1723     1727       +4     
==========================================
  Hits         1657     1657              
- Misses        810      813       +3     
- Partials     1182     1184       +2     

Flag	Coverage Δ
unittests	45.34% <0.00%> (-0.07%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	0.00% <ø> (ø)
...ory_controller/src/joint_trajectory_controller.cpp	46.94% <0.00%> (-0.33%)	⬇️

[christophfroehlich]

Looks good for me (even more if the mentioned tests are added)
concerning closed-loop mode: I'm not sure if this is necessary, shouldn't the trajectory generator be aware of the current state? But I guess there might be configurations where this could improve something.

[destogl]

concerning closed-loop mode: I'm not sure if this is necessary, shouldn't the trajectory generator be aware of the current state? But I guess there might be configurations where this could improve something.

This is important only when trajectory replacement is done. My comment was along: "if strange behavior happens when replacing trajectories - it might be that the state of is too

[christophfroehlich]

I'm asking myself if it is desirable at all to interpolate from measured state instead of the last command with closed-loop config. but that really depends on the algorithm generating the trajectory

[mergify]

This pull request is in conflict. Could you fix it @destogl?

[christophfroehlich]

test_no_jump_when_state_tracking_error_not_updated fails

[mergify]

This pull request is in conflict. Could you fix it @destogl?