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 89.18919% with 4 lines in your changes missing coverage. Please review.
✅ Project coverage is 85.17%. Comparing base (cbc948e) to head (55d5abc).

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

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2294      +/-   ##
==========================================
+ Coverage   85.16%   85.17%   +0.01%     
==========================================
  Files         154      154              
  Lines       15463    15436      -27     
  Branches     1334     1330       -4     
==========================================
- Hits        13169    13148      -21     
+ Misses       1803     1800       -3     
+ Partials      491      488       -3     

Flag	Coverage Δ
unittests	85.17% <89.18%> (+0.01%)	⬆️

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.73% <100.00%> (ø)
...ory_controller/src/joint_trajectory_controller.cpp	83.94% <87.87%> (+0.18%)	⬆️

🚀 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?