[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 96.72131% with 2 lines in your changes missing coverage. Please review.

Project coverage is 80.38%. Comparing base (97c1e24) to head (c4022a2).

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

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1306      +/-   ##
==========================================
+ Coverage   80.34%   80.38%   +0.03%     
==========================================
  Files         105      105              
  Lines        9384     9402      +18     
  Branches      826      829       +3     
==========================================
+ Hits         7540     7558      +18     
- Misses       1570     1571       +1     
+ Partials      274      273       -1     

Flag	Coverage Δ
unittests	80.38% <96.72%> (+0.03%)	⬆️

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% <ø> (ø)
...ory_controller/test/test_trajectory_controller.cpp	99.74% <100.00%> (+<0.01%)	⬆️
...ntroller/test/test_trajectory_controller_utils.hpp	84.15% <100.00%> (ø)
...ory_controller/src/joint_trajectory_controller.cpp	83.67% <71.42%> (-0.18%)	⬇️

... and 2 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

[saikishor]

Above you are doing the initial sampling and immediately overriding with this future one. Is this intended?

[fmauch]

Yes. The sample call above is - as the comment says - only to establish the actual start time of the trajectory. However, also in the first control cycle when we started the trajectory we want to sample at $t+\Delta t$ to generate the commands.

This should be probably refactored in the trajectory representation that the start time would not implicitly be specified by the first sample, but I thought this might not be the right PR to address that...

[saikishor]

shouldn't this use the period itself?
I think the period is more correct than the update_period_

[fmauch]

It would usually not make that big of a difference, but isn't the goal here, as well, to generate a command to reach state_desired at the end of the current control cycle (which will be update_period_ in the future).

[christophfroehlich]

Actually, I agree with Sai here: This is not related to the trajectory sampling but only for calculating the feedback control law (in fact, only for integrating the error for the integral part of the command).

[saikishor]

If the control period is 100 milliseconds, shouldn't it be 350?
Am i missing something here?

[fmauch]

Why? Would it be desired to set a target time between two control cycles?

With a control period of 100 ms we would call update at $t_0 + 100ms, t_0 + 200ms, t_0+300ms$... So, in the update call at $t=t_0+200ms$ it should generate the command to reach the setpoint which is what we wait for below.

[christophfroehlich]

Is there a reason that you don't call waitAndCompareState() here? why is now traj_controller_->has_position_command_interface() necessary, but wasn't before?

[fmauch]

If you look at the implementation of waitAndCompareState, you'll see that it is almost equivalent:

    traj_controller_->wait_for_trajectory(executor);
    auto end_time = updateControllerAsync(controller_wait_time, start_time);

    // get states from class variables
    auto state_feedback = traj_controller_->get_state_feedback();
    auto state_reference = traj_controller_->get_state_reference();

    for (size_t i = 0; i < expected_actual.positions.size(); ++i)
    {
      SCOPED_TRACE("Joint " + std::to_string(i));
      // TODO(anyone): add checking for velocities and accelerations
      if (traj_controller_->has_position_command_interface())
      {
        EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], allowed_delta);
      }
    }

    for (size_t i = 0; i < expected_desired.positions.size(); ++i)
    {
      SCOPED_TRACE("Joint " + std::to_string(i));
      // TODO(anyone): add checking for velocities and accelerations
      if (traj_controller_->has_position_command_interface())
      {
        EXPECT_NEAR(expected_desired.positions[i], state_reference.positions[i], allowed_delta);
      }
    }

• I preferred giving a specific update rate to the updateControllerAsync function that matches the actual update rate.
• The waitAndCompareState function only checks positions, hence I also only checked positions. So, it wasn't necessary before, because it was inside waitAndCompareState.
• More importantly, waitAndCompareState checks for the actual position and the generated position command. However, due to spline interpolation we don't necessarily know the generated command. Assuming expected_desired = expected_actual; is simply wrong as long as the robot is in motion (which it is, continuing to the second waypoint). It did work, more or less by accident, since the robot is initialized at {1.2, 2.1, 3.1} and moves to {2.0, 3.0, 4.0} in the first segment. Hence, the trajectory "arrives" at the setpoint quite slowly. However, in the next segment (which has the same time), it is supposed to go to {4.0, 5.0, 6.0}, so it has to move much faster in that segment. Before my change, sampling was basically moved back one cycle, so the desired state was 2.0 for joint1 which matches the desired state and the actual state was something right before that, which was in the range of the 0.1 tolerance (which is probably the reason why it was so large). However, after the changes from this PR, the actual state will be at 2.0 at this time and the desired will be on the way towards 4.0 already. (In fact, it will be at 2.3599999999999999). If we move sampling back by one control cycle to rclcpp::Duration(delay) - rclcpp::Duration::from_seconds(0.1) the desired state matches 2.0 exactly, while the actual state is 1.982, which falls into the 0.1 range. Since the comment to that check says Check that we reached end of points_old[0]trajectory I thought it would be better to check exactly that with a small tolerance instead of having to increase the tolerance to actually not fail a check that doesn't only check what we want to know at that point. I hope, that paragraph makes sense...

[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.