Skip to content

Fix dynamics calculation of GenericSystem component #2705

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
saikishor merged 9 commits into from
Oct 19, 2025
Merged

Fix dynamics calculation of GenericSystem component #2705

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
94c690b
Add two currently failing tests
christophfroehlich Oct 18, 2025
ce063d1
Don't try to set unavailable states
christophfroehlich Oct 18, 2025
10cefda
Fix test criteria
christophfroehlich Oct 18, 2025
b8a426b
Do real backward Euler method
christophfroehlich Oct 18, 2025
45971be
Improve test coverage with position_state_following_offset
christophfroehlich Oct 18, 2025
ce5fc19
Fix typo
christophfroehlich Oct 18, 2025
a479770
Robustify tests
christophfroehlich Oct 18, 2025
7baa5ef
Improve test coverage
christophfroehlich Oct 18, 2025
a506539
Fix comments and cleanup
christophfroehlich Oct 18, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 23 additions & 22 deletions hardware_interface/src/mock_components/generic_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -427,50 +427,49 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
{
case ACCELERATION_INTERFACE_INDEX:
{
// currently we do backward integration
joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only
std::isfinite(joint_state_values_[VELOCITY_INTERFACE_INDEX])
? joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds()
: 0.0 + (custom_interface_with_following_offset_.empty()
? position_state_following_offset_
: 0.0);

if (std::isnan(joint_state_values_[VELOCITY_INTERFACE_INDEX]))
{
joint_state_values_[VELOCITY_INTERFACE_INDEX] = 0.0;
}
joint_state_values_[VELOCITY_INTERFACE_INDEX] +=
std::isnan(joint_state_values_[ACCELERATION_INTERFACE_INDEX])
? 0.0
: joint_state_values_[ACCELERATION_INTERFACE_INDEX] * period.seconds();

if (std::isfinite(joint_command_values_[ACCELERATION_INTERFACE_INDEX]))
{
joint_state_values_[ACCELERATION_INTERFACE_INDEX] =
joint_command_values_[ACCELERATION_INTERFACE_INDEX];
}
break;
}
case VELOCITY_INTERFACE_INDEX:
{
// currently we do backward integration
// currently we do backward Euler integration
joint_state_values_[VELOCITY_INTERFACE_INDEX] +=
std::isnan(joint_state_values_[ACCELERATION_INTERFACE_INDEX])
? 0.0
: joint_state_values_[ACCELERATION_INTERFACE_INDEX] * period.seconds();
joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only
std::isfinite(joint_state_values_[VELOCITY_INTERFACE_INDEX])
? joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds()
: 0.0 + (custom_interface_with_following_offset_.empty()
? position_state_following_offset_
: 0.0);

break;
}
case VELOCITY_INTERFACE_INDEX:
{
if (std::isfinite(joint_command_values_[VELOCITY_INTERFACE_INDEX]))
{
const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX];

const double old_velocity = std::isfinite(joint_state_values_[VELOCITY_INTERFACE_INDEX])
? joint_state_values_[VELOCITY_INTERFACE_INDEX]
: 0.0;
joint_state_values_[VELOCITY_INTERFACE_INDEX] =
joint_command_values_[VELOCITY_INTERFACE_INDEX];

joint_state_values_[ACCELERATION_INTERFACE_INDEX] =
(joint_state_values_[VELOCITY_INTERFACE_INDEX] - old_velocity) / period.seconds();
}
// currently we do backward Euler integration
joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only
std::isfinite(joint_state_values_[VELOCITY_INTERFACE_INDEX])
? joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds()
: 0.0 + (custom_interface_with_following_offset_.empty()
? position_state_following_offset_
: 0.0);
break;
}
case POSITION_INTERFACE_INDEX:
Expand All @@ -497,9 +496,11 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
}
}
// mirror them back
for (size_t i = 0; i < 3; ++i)
for (size_t i = 0; i < standard_interfaces_.size(); ++i)
{
if (std::isfinite(joint_state_values_[i]))
if (
std::isfinite(joint_state_values_[i]) &&
has_state(joint_name + "/" + standard_interfaces_[i]))
{
set_state(joint_name + "/" + standard_interfaces_[i], joint_state_values_[i]);
}
Expand Down
Loading
Loading