More general initialization of state from command

[peter-mitrano-ar]

Description

When operating a robot which does not always track desired positions accurately (e.g. torque controlled) you may sometimes want the JTC to initialize based on the current/latest command from the hardware interface, rather than the current measured state. This will ensure the "command" is unchanged when activating the controller. Otherwise, if you use measured state, the arm can move unexpectedly (usually, it will droop).

To handle this, JTC has set_last_command_interface_value_as_state_on_activation parameter. However, it's implementation is weird to me. Specifically, bool read_state_from_command_interfaces will return False if you have a velocity state interface but no velocity command interface. For me, this restriction seems unnecessary. You simply should need a state interface for each command interface, but not the other way around. Extra state interfaces are simply ignored.

Explanation of changes

Now, we always first call read_state_from_state_interfaces in on_activate.
Then, if the parameter is set, we update this state from the command interfaces.
Thus, we renamed read_state_from_command_interfaces to update_state_from_command_interface and change its behavior to only update fields that have command interfaces.
To minimize code duplication, I refactored out interface_has_values and assign_point_from_command_interface. This is the reason we have more deletions than additions :)

Also I updated the tests to reflect the desired change in behavior.

Fixes # (issue)

https://discourse.openrobotics.org/t/handling-missing-command-interfaces-in-read-state-from-command-interface/53551

Is this user-facing behavior change?

Yes!

Did you use Generative AI?

A bit of copilot autocompletion.

[christophfroehlich]

Your description seems fair for me. Can you please also improve the parameter description in the yaml file, and add a line to the release_notes?

[codecov]

Codecov Report

❌ Patch coverage is 94.11765% with 3 lines in your changes missing coverage. Please review.
✅ Project coverage is 84.97%. Comparing base (27ea539) to head (43550aa).
⚠️ Report is 3 commits behind head on master.

Files with missing lines	Patch %	Lines
...ory_controller/src/joint_trajectory_controller.cpp	90.90%	1 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2294      +/-   ##
==========================================
- Coverage   84.99%   84.97%   -0.02%     
==========================================
  Files         145      145              
  Lines       15186    14996     -190     
  Branches     1331     1305      -26     
==========================================
- Hits        12907    12743     -164     
+ Misses       1789     1783       -6     
+ Partials      490      470      -20     

Flag	Coverage Δ
unittests	84.97% <94.11%> (-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	50.00% <ø> (ø)
...ory_controller/test/test_trajectory_controller.cpp	99.75% <100.00%> (+0.01%)	⬆️
...ory_controller/src/joint_trajectory_controller.cpp	83.96% <90.90%> (+0.29%)	⬆️

... and 22 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[saikishor]

Is there a way to add a test to this usecase?

[peter-mitrano-ar]

Thanks for the quick replies everyone!
@christophfroehlich updated the docs/comments
@saikishor I added a new test for this parameter. I also confirmed (locally) that if I set the parameter to false, the test fails which is good 😄

In thinking about these features more, I wonder whether it really makes sense to initialize state_current_ to command interfaces. I definitely want last_command_state_ set correctly, and ultimate what I want is that when you first update the commands, you set them to the command not the measured state. The tests seem to be only testing internal state of the controller (last_commanded_state_), not what actually gets bound to the hardware interface (I think joint_command_interface_). Does anyone have thoughts on tihs?

[peter-mitrano-ar]

Not sure about the CI failures, looks related to this but not sure if I did something wrong or need to somehow update something to get CI to pass.

[christophfroehlich]

Not sure about the CI failures, looks related to this but not sure if I did something wrong or need to somehow update something to get CI to pass.

tl;dr not related ;)

You are right. I already released ros2_control, but the build farm had an outtake this week and the binaries are still not available on ros-testing. Windows build fails because of the removal done by #2016 until we have released this.

[peter-mitrano-ar]

@destogl since it seems you wrote the initial implementation, maybe you want to check that this logic change makes sense

[christophfroehlich]

LGTM

[peter-mitrano-ar]

should I rebase on master? or will that trigger needless CI and/or dismiss your approval?

[christophfroehlich]

Please never do a rebase if there was already any review in the PR, see the hint in the PR template you certainly have read ;)
Updating via merge via UI without having merge conflicts normally doesn't dismiss approvals. But let's wait for another review, then re-trigger CI before merging.