[JTC] Sample at t + dT instead of the beginning of the control cycle

[fmauch]

As mentioned in #1191 (comment) and discussed in #697 the JTC currently samples the trajectory at the time given to the update(time, period) method. However, this actually is the beginning of the control cycle and it would make more sense to sample the setpoint for the end of the control cycle, resulting in time + controller_update_period. This PR implements that.

I obviously had to update a couple of tests on the way, since with changing the sampling point, the resulting command can be significantly different.

[codecov]

Codecov Report

Attention: Patch coverage is 97.29730% with 2 lines in your changes missing coverage. Please review.

Project coverage is 80.92%. Comparing base (f519170) to head (353f664).
Report is 2 commits behind head on master.

Files with missing lines	Patch %	Lines
...ory_controller/src/joint_trajectory_controller.cpp	85.71%	1 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1306      +/-   ##
==========================================
+ Coverage   80.89%   80.92%   +0.02%     
==========================================
  Files         109      109              
  Lines        9696     9723      +27     
  Branches      842      850       +8     
==========================================
+ Hits         7844     7868      +24     
- Misses       1574     1575       +1     
- Partials      278      280       +2     

Flag	Coverage Δ
unittests	80.92% <97.29%> (+0.02%)	⬆️

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

Files with missing lines	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	100.00% <ø> (ø)
...include/joint_trajectory_controller/trajectory.hpp	75.00% <ø> (ø)
joint_trajectory_controller/src/trajectory.cpp	91.35% <100.00%> (+0.04%)	⬆️
...ory_controller/test/test_trajectory_controller.cpp	99.74% <100.00%> (+<0.01%)	⬆️
...ntroller/test/test_trajectory_controller_utils.hpp	84.14% <100.00%> (ø)
...ory_controller/src/joint_trajectory_controller.cpp	83.90% <85.71%> (-0.13%)	⬇️

... and 3 files with indirect coverage changes

[saikishor]

Look good. I've left some questions
I'll continue reviewing after we discuss this part.

Thanks for your great work @fmauch

[fmauch]

The changes from ros-controls/ros2_control#1788 require revisiting the tests from this. I expect, that tests on the master branch will fail, as well, however, since I had to set correct rates all over the place within this PR.

Well, maybe not. I'll check this one, though.

Edit: Things are back to normal now.

[saikishor]

LGTM

[christophfroehlich]

I just read once more the linked issue #697 and I think he is right in this point:

However JTC in ros_controllers is comparing current state with future state (at T + dt), and this seems wrong. This will always lead to a tracking or goal error what ever perfect robot (mirrored cmd/state ifs) there is.

This is exactly what is done in this PR here?

[fmauch]

In 0d00727 I implemented using the trajectory sample point at time T as the desired_state. This is used for checking the tolerances, generating the feedback's desired state and error and to compute the PID value for the closed loop controller. I am unsure about the latter, as it was also discussed in #697.

[christophfroehlich]

In 0d00727 I implemented using the trajectory sample point at time T as the desired_state. This is used for checking the tolerances, generating the feedback's desired state and error and to compute the PID value for the closed loop controller. I am unsure about the latter, as it was also discussed in #697.

I think this is a proper way in handling this, and it's a minimal change in the current behavior by now.
But I'm wondering if there is a problem with #1297: It may happen that the sampling at t_2 of the following update method will sample at a time before t_1+dt?

[fmauch]

In 0d00727 I implemented using the trajectory sample point at time T as the desired_state. This is used for checking the tolerances, generating the feedback's desired state and error and to compute the PID value for the closed loop controller. I am unsure about the latter, as it was also discussed in #697.

I think this is a proper way in handling this, and it's a minimal change in the current behavior by now. But I'm wondering if there is a problem with #1297: It may happen that the sampling at t_2 of the following update method will sample at a time before t_1+dt?

Yes, this may definitively happen, especially once we actually get to time-scaling as it is my overall target with this. I'm not entierly familiar with #1297, so I'll have to read into that.

[fmauch]

Yes, this may definitively happen, especially once we actually get to time-scaling as it is my overall target with this. I'm not entierly familiar with #1297, so I'll have to read into that.

Coming back to this: Yes, this is definitively a problem. One solution could be to make the index progression in the sample method optional such that it could only be moving forwards for the sampling at time T, but not at sampling for T+dT.

---

Edit: Implemented in 353f664

[christophfroehlich]

That's fine, I also thought about fixing it like this. ABI break should be fine for the trajectory class.