diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 6fd7673cbe..61bb917074 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -740,7 +740,7 @@ class ResourceStorage std::vector soft_limits; const std::vector hard_limits{limits}; joint_limits::JointInterfacesCommandLimiterData data; - data.joint_name = joint_name; + data.set_joint_name(joint_name); limiters_data_.insert({joint_name, data}); // If the joint limits is found in the softlimits, then extract it if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end()) @@ -1025,7 +1025,7 @@ class ResourceStorage { is_limited = false; joint_limits::JointInterfacesCommandLimiterData data; - data.joint_name = joint_name; + data.set_joint_name(joint_name); update_joint_limiters_data(data.joint_name, state_interface_map_, data.actual); if (interface_name == hardware_interface::HW_IF_POSITION) { diff --git a/joint_limits/include/joint_limits/data_structures.hpp b/joint_limits/include/joint_limits/data_structures.hpp index 3b33a972b7..b1c4dd1140 100644 --- a/joint_limits/include/joint_limits/data_structures.hpp +++ b/joint_limits/include/joint_limits/data_structures.hpp @@ -70,6 +70,7 @@ struct JointControlInterfacesData std::string to_string() const { std::string str; + str += "Joint: '" + joint_name + "', "; if (has_position()) { str += "position: " + std::to_string(position.value()) + ", "; @@ -117,6 +118,15 @@ struct JointInterfacesCommandLimiterData bool has_limited_data() const { return limited.has_data(); } + void set_joint_name(const std::string & name) + { + joint_name = name; + actual.joint_name = name; + command.joint_name = name; + limited.joint_name = name; + prev_command.joint_name = name; + } + std::string to_string() const { return fmt::format( diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 3af2d313ad..b879d4e316 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -32,6 +32,14 @@ constexpr double POSITION_BOUNDS_TOLERANCE = 0.002; constexpr double OUT_OF_BOUNDS_EXCEPTION_TOLERANCE = 0.0087; } // namespace internal +/** + * @brief Updates the previous command with the desired command. + * @param prev_command The previous command to update. + * @param desired The desired command which is limited. + */ +void update_prev_command( + const JointControlInterfacesData & desired, JointControlInterfacesData & prev_command); + /** * @brief Checks if a value is limited by the given limits. * @param value The value to check. diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index c4ebe819ad..5a49c7ba88 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -72,6 +72,32 @@ void verify_actual_position_within_limits( } } // namespace internal +void update_prev_command( + const JointControlInterfacesData & desired, JointControlInterfacesData & prev_command) +{ + if (desired.has_position()) + { + prev_command.position = desired.position; + } + if (desired.has_velocity()) + { + prev_command.velocity = desired.velocity; + } + if (desired.has_effort()) + { + prev_command.effort = desired.effort; + } + if (desired.has_acceleration()) + { + prev_command.acceleration = desired.acceleration; + } + if (desired.has_jerk()) + { + prev_command.jerk = desired.jerk; + } + prev_command.joint_name = desired.joint_name; +} + bool is_limited(double value, double min, double max) { return value < min || value > max; } PositionLimits compute_position_limits( diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 2baa848cbc..b5e13c538c 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -61,45 +61,26 @@ bool JointSaturationLimiter::on_enforce( // The following conditional filling is needed for cases of having certain information missing if (!prev_command_.has_data()) { - if (actual.has_position()) + if (desired.has_position()) { - prev_command_.position = actual.position; + prev_command_.position = actual.has_position() ? actual.position : desired.position; } - else if (desired.has_position()) + if (desired.has_velocity()) { - prev_command_.position = desired.position; + prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity; } - if (actual.has_velocity()) + if (desired.has_effort()) { - prev_command_.velocity = actual.velocity; + prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort; } - else if (desired.has_velocity()) + if (desired.has_acceleration()) { - prev_command_.velocity = desired.velocity; + prev_command_.acceleration = + actual.has_acceleration() ? actual.acceleration : desired.acceleration; } - if (actual.has_effort()) + if (desired.has_jerk()) { - prev_command_.effort = actual.effort; - } - else if (desired.has_effort()) - { - prev_command_.effort = desired.effort; - } - if (actual.has_acceleration()) - { - prev_command_.acceleration = actual.acceleration; - } - else if (desired.has_acceleration()) - { - prev_command_.acceleration = desired.acceleration; - } - if (actual.has_jerk()) - { - prev_command_.jerk = actual.jerk; - } - else if (desired.has_jerk()) - { - prev_command_.jerk = desired.jerk; + prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk; } if (actual.has_data()) { @@ -159,7 +140,7 @@ bool JointSaturationLimiter::on_enforce( desired.jerk = std::clamp(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk); } - prev_command_ = desired; + update_prev_command(desired, prev_command_); return limits_enforced; } diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index 07f943dfa8..ff86a00765 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -44,45 +44,26 @@ bool JointSoftLimiter::on_enforce( if (!prev_command_.has_data()) { - if (actual.has_position()) + if (desired.has_position()) { - prev_command_.position = actual.position; + prev_command_.position = actual.has_position() ? actual.position : desired.position; } - else if (desired.has_position()) + if (desired.has_velocity()) { - prev_command_.position = desired.position; + prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity; } - if (actual.has_velocity()) + if (desired.has_effort()) { - prev_command_.velocity = actual.velocity; + prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort; } - else if (desired.has_velocity()) + if (desired.has_acceleration()) { - prev_command_.velocity = desired.velocity; + prev_command_.acceleration = + actual.has_acceleration() ? actual.acceleration : desired.acceleration; } - if (actual.has_effort()) + if (desired.has_jerk()) { - prev_command_.effort = actual.effort; - } - else if (desired.has_effort()) - { - prev_command_.effort = desired.effort; - } - if (actual.has_acceleration()) - { - prev_command_.acceleration = actual.acceleration; - } - else if (desired.has_acceleration()) - { - prev_command_.acceleration = desired.acceleration; - } - if (actual.has_jerk()) - { - prev_command_.jerk = actual.jerk; - } - else if (desired.has_jerk()) - { - prev_command_.jerk = desired.jerk; + prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk; } if (actual.has_data()) { @@ -266,7 +247,7 @@ bool JointSoftLimiter::on_enforce( limits_enforced = true; } - prev_command_ = desired; + update_prev_command(desired, prev_command_); return limits_enforced; } diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp index 7b0227602e..8b8d865810 100644 --- a/joint_limits/test/test_joint_soft_limiter.cpp +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -548,6 +548,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) limits.max_effort = 200.0; joint_limits::SoftJointLimits soft_limits; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); // Reset the desired and actual states @@ -651,6 +652,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) // Check with high values of k_velocity (hard limits should be considered) soft_limits.k_velocity = 5000.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); for (auto act_pos : @@ -674,6 +676,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) // Check with low values of k_velocity soft_limits.k_velocity = 300.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); for (auto act_pos : @@ -702,6 +705,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) soft_limits.min_position = -4.0; soft_limits.max_position = 4.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); test_limit_enforcing(0.0, 0.5, 20.0, 20.0, false); @@ -762,6 +766,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) soft_limits.min_position = -10.0; soft_limits.max_position = 10.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); for (auto act_pos : @@ -788,6 +793,7 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) soft_limits.min_position = -10.0; soft_limits.max_position = 10.0; ASSERT_TRUE(Init(limits, soft_limits)); + last_commanded_state_ = {}; ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); for (auto act_pos :