Skip to content

More general initialization of state from command#2294

Merged
saikishor merged 14 commits into
ros-controls:masterfrom
peter-mitrano-ar:partial-init-from-commanded-state-simpler
May 14, 2026
Merged

More general initialization of state from command#2294
saikishor merged 14 commits into
ros-controls:masterfrom
peter-mitrano-ar:partial-init-from-commanded-state-simpler

Conversation

@peter-mitrano-ar
Copy link
Copy Markdown
Contributor

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.

@peter-mitrano-ar peter-mitrano-ar changed the title Partial init from commanded state simpler More general initialization of state from command Apr 13, 2026
Copy link
Copy Markdown
Member

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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
Copy link
Copy Markdown

codecov Bot commented Apr 13, 2026

Codecov Report

❌ Patch coverage is 94.11765% with 3 lines in your changes missing coverage. Please review.
✅ Project coverage is 85.19%. Comparing base (e3c31f1) to head (7a83fa2).

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   85.16%   85.19%   +0.03%     
==========================================
  Files         145      145              
  Lines       15407    15394      -13     
  Branches     1322     1322              
==========================================
- Hits        13121    13115       -6     
+ Misses       1797     1794       -3     
+ Partials      489      485       -4     
Flag Coverage Δ
unittests 85.19% <94.11%> (+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 40.00% <ø> (ø)
...ory_controller/test/test_trajectory_controller.cpp 99.34% <100.00%> (+<0.01%) ⬆️
...ory_controller/src/joint_trajectory_controller.cpp 83.98% <90.90%> (+0.29%) ⬆️
🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Copy link
Copy Markdown
Member

@saikishor saikishor left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@peter-mitrano-ar
Copy link
Copy Markdown
Contributor Author

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
Copy link
Copy Markdown
Contributor Author

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
Copy link
Copy Markdown
Member

christophfroehlich commented Apr 26, 2026

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.

Comment thread doc/release_notes.rst Outdated
Comment thread joint_trajectory_controller/test/test_trajectory_controller.cpp
add link to PR

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
@peter-mitrano-ar
Copy link
Copy Markdown
Contributor Author

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

Copy link
Copy Markdown
Member

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

@peter-mitrano-ar
Copy link
Copy Markdown
Contributor Author

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

@christophfroehlich
Copy link
Copy Markdown
Member

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.

@peter-mitrano-ar
Copy link
Copy Markdown
Contributor Author

friendly ping in hopes of a review @saikishor

Copy link
Copy Markdown
Member

@saikishor saikishor left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Apologies for not getting to this PR before

The changes in general look good to me. Just 2 minor nitpicks.

Let me know what you think.

Comment thread joint_trajectory_controller/src/joint_trajectory_controller.cpp Outdated
Comment thread joint_trajectory_controller/src/joint_trajectory_controller.cpp Outdated
Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
@peter-mitrano-ar
Copy link
Copy Markdown
Contributor Author

@saikishor thanks! Changes included.
@christophfroehlich since review was dismissed

@peter-mitrano-ar
Copy link
Copy Markdown
Contributor Author

I clicked "rerequest review", since it looked like the new commit means old approvals are dismissed. Sorry if that generated extra emails/notifications!

@saikishor saikishor merged commit f6ee8cb into ros-controls:master May 14, 2026
17 of 31 checks passed
@github-project-automation github-project-automation Bot moved this from Needs review to Done in Review triage May 14, 2026
@PeterMitrano
Copy link
Copy Markdown

@saikishor is there a bot that can backport to Jazzy/Kilted...? I am planning to do a backport to Humble but I am pretty sure that has to be manual since the code has changed a lot.

@saikishor
Copy link
Copy Markdown
Member

@saikishor is there a bot that can backport to Jazzy/Kilted...? I am planning to do a backport to Humble but I am pretty sure that has to be manual since the code has changed a lot.

I think there shouldn't be problem backporting it, as it only affects the case when the interface is only available as a command but not as a state. @christophfroehlich what do you think?

@christophfroehlich christophfroehlich added backport-jazzy Triggers PR backport to ROS 2 jazzy. backport-kilted Triggers PR backport to ROS 2 kilted. labels May 15, 2026
@peter-mitrano-ar
Copy link
Copy Markdown
Contributor Author

Hmm slight correction -- I believe it effects also the case where you have the interface only as state and not command. That's how I discovered this in the first place. At least in humble, if you had position command but position,velocity state, the controller would initialize from the state not command.

@christophfroehlich
Copy link
Copy Markdown
Member

do you need it on humble? if it is too much effort, I would not backport it
users can always compile the rolling version on humble if needed

@peter-mitrano-ar
Copy link
Copy Markdown
Contributor Author

I would like it in humble. It shouldn't take more than an hour for me to do the backport. My current workaround, unbelievably, is to remove my velocity state interface 😆

Compiling rolling on humble is possible, yes, but I think I wouldn't do it. It involves also bringing many other repos (ros2 control, ros2 control cmake, pal statistics, ...) from rolling as well.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

backport-jazzy Triggers PR backport to ROS 2 jazzy. backport-kilted Triggers PR backport to ROS 2 kilted.

Projects

Status: Done

Development

Successfully merging this pull request may close these issues.

4 participants