From d3e9be21c97a63c193ce3fb9e7ba0601ea727c48 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 Jan 2025 18:52:21 +0100 Subject: [PATCH 1/5] use the actual position to limit the desired position --- .../joint_limits/joint_limits_helpers.hpp | 2 +- joint_limits/src/joint_limits_helpers.cpp | 21 ++++++- joint_limits/src/joint_range_limiter.cpp | 4 +- joint_limits/src/joint_soft_limiter.cpp | 4 +- .../test/test_joint_range_limiter.cpp | 55 +++++++++++++++++-- 5 files changed, 74 insertions(+), 12 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 8a17896139..b84c2662c8 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -49,7 +49,7 @@ bool is_limited(double value, double min, double max); */ PositionLimits compute_position_limits( const joint_limits::JointLimits & limits, const std::optional & act_vel, - const std::optional & prev_command_pos, double dt); + const std::optional & act_pos, const std::optional & prev_command_pos, double dt); /** * @brief Computes the velocity limits based on the position and acceleration limits. diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index 11ccaf7738..ac02ca7de9 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -39,7 +39,7 @@ bool is_limited(double value, double min, double max) { return value < min || va PositionLimits compute_position_limits( const joint_limits::JointLimits & limits, const std::optional & act_vel, - const std::optional & prev_command_pos, double dt) + const std::optional & act_pos, const std::optional & prev_command_pos, double dt) { PositionLimits pos_limits(limits.min_position, limits.max_position); if (limits.has_velocity_limits) @@ -50,8 +50,23 @@ PositionLimits compute_position_limits( : limits.max_velocity; const double max_vel = std::min(limits.max_velocity, delta_vel); const double delta_pos = max_vel * dt; - pos_limits.lower_limit = std::max(prev_command_pos.value() - delta_pos, pos_limits.lower_limit); - pos_limits.upper_limit = std::min(prev_command_pos.value() + delta_pos, pos_limits.upper_limit); + const double position_reference = + act_pos.has_value() ? act_pos.value() : prev_command_pos.value(); + pos_limits.lower_limit = std::max(position_reference - delta_pos, pos_limits.lower_limit); + pos_limits.upper_limit = std::min(position_reference + delta_pos, pos_limits.upper_limit); + RCLCPP_ERROR_EXPRESSION( + rclcpp::get_logger("joint_limiter_interface"), + act_pos.has_value(), "Joint position limits computed based on the actual position : [%.5f, " + "%.5f].", position_reference, prev_command_pos.value()); + RCLCPP_ERROR_EXPRESSION( + rclcpp::get_logger("joint_limiter_interface"), + !act_pos.has_value(), "Didn't use the actual position to compute the limits.: [%.5f, " + "%.5f].", position_reference, prev_command_pos.value()); + RCLCPP_ERROR_EXPRESSION( + rclcpp::get_logger("joint_limiter_interface"), + act_pos.has_value() || prev_command_pos.has_value(), + "Joint position limits computed based on velocity limits: [%.5f, %.5f].", + pos_limits.lower_limit, pos_limits.upper_limit); } internal::check_and_swap_limits(pos_limits.lower_limit, pos_limits.upper_limit); return pos_limits; diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 2279b910ae..ae9c4fe507 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -112,8 +112,8 @@ bool JointSaturationLimiter::on_enforce( if (desired.has_position()) { - const auto limits = - compute_position_limits(joint_limits, actual.velocity, prev_command_.position, dt_seconds); + const auto limits = compute_position_limits( + joint_limits, actual.velocity, actual.position, prev_command_.position, dt_seconds); limits_enforced = is_limited(desired.position.value(), limits.lower_limit, limits.upper_limit); desired.position = std::clamp(desired.position.value(), limits.lower_limit, limits.upper_limit); } diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index a2292cb033..432edb5437 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -141,8 +141,8 @@ bool JointSoftLimiter::on_enforce( if (desired.has_position()) { - const auto position_limits = - compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); + const auto position_limits = compute_position_limits( + hard_limits, actual.velocity, actual.position, prev_command_.position, dt_seconds); double pos_low = -std::numeric_limits::infinity(); double pos_high = std::numeric_limits::infinity(); diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 083646d84f..41ab569289 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -142,6 +142,32 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) EXPECT_FALSE(desired_state_.has_effort()); EXPECT_FALSE(desired_state_.has_jerk()); + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 0.0; + desired_state_.position = 5.0 * M_PI; + const rclcpp::Duration test_period(0, 100); // 0.1 second + for (size_t i = 0; i < 2000; i++) + { + desired_state_.position = 5.0 * M_PI; + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(actual_state_.position.value()) + + ", desired position: " + std::to_string(desired_state_.position.value()) + + " for the joint limits : " + limits.to_string() + " Iteration : " + std::to_string(i)); + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, test_period)); + EXPECT_NEAR( + desired_state_.position.value(), + std::min( + actual_state_.position.value() + (limits.max_velocity * test_period.seconds()), M_PI), + COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + actual_state_.position = desired_state_.position.value() / 2.0; + } + // Now test when there are no position limits, then the desired position is not saturated limits = joint_limits::JointLimits(); ASSERT_TRUE(Init(limits)); @@ -652,19 +678,40 @@ TEST_F(JointSaturationLimiterTest, check_all_desired_references_limiting) test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 1.0, 0.5, 0.5, 3.0, 1.0, 0.5, 0.5, false); + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(std::nullopt, std::nullopt, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.5, 0.5, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 3.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 4.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + // Now enforce the limits with actual position and velocity ASSERT_TRUE(Init(limits)); // Desired position and velocity affected due to the acceleration limits test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); - test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); - test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); - test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.2, 0.0, 6.0, 0.0, 0.0, 0.0, 1.7, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.5, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.5, 0.0, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.5, 0.5, 0.5, 0.5, true); test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); - test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.0, 1.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(4.8, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.5, 0.5, 0.5, true); test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true); + test_limit_enforcing(5.0, 1.0, 6.0, 3.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true); } int main(int argc, char ** argv) From 4d4ec17dfc2a19d520f34875f0e5eaefc9af83e7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 Jan 2025 19:28:41 +0100 Subject: [PATCH 2/5] Use const for the current joint state --- .../joint_limits/joint_limiter_interface.hpp | 4 +- .../joint_limits/joint_saturation_limiter.hpp | 2 +- .../joint_limits/joint_soft_limiter.hpp | 2 +- joint_limits/src/joint_range_limiter.cpp | 2 +- joint_limits/src/joint_saturation_limiter.cpp | 44 ++++++++----------- joint_limits/src/joint_soft_limiter.cpp | 2 +- 6 files changed, 25 insertions(+), 31 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index cbe42999ab..27883b8937 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -201,7 +201,7 @@ class JointLimiterInterface * \returns true if limits are enforced, otherwise false. */ virtual bool enforce( - JointLimitsStateDataType & current_joint_states, + const JointLimitsStateDataType & current_joint_states, JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) { joint_limits_ = *(updated_limits_.readFromRT()); @@ -234,7 +234,7 @@ class JointLimiterInterface * \returns true if limits are enforced, otherwise false. */ virtual bool on_enforce( - JointLimitsStateDataType & current_joint_states, + const JointLimitsStateDataType & current_joint_states, JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; /** \brief Checks if the logging interface is set. diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 9af574078d..3b7451e2ee 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -67,7 +67,7 @@ class JointSaturationLimiter : public JointLimiterInterface::on_init() template <> bool JointSaturationLimiter::on_enforce( - JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const JointControlInterfacesData & actual, JointControlInterfacesData & desired, const rclcpp::Duration & dt) { bool limits_enforced = false; diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 47a88e658d..e61cb429ec 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -27,7 +27,7 @@ namespace joint_limits { template <> bool JointSaturationLimiter::on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { bool limits_enforced = false; @@ -44,23 +44,21 @@ bool JointSaturationLimiter::on_enfo // velocity max is implicitly already violated due to max_acc * dt > 2.0 // check for required inputs combination - bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); - bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_); - bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_); - bool has_current_position = (current_joint_states.positions.size() == number_of_joints_); - bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_); + const bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); + const bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_); + const bool has_desired_acceleration = + (desired_joint_states.accelerations.size() == number_of_joints_); + const bool has_current_position = (current_joint_states.positions.size() == number_of_joints_); + const bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_); // pos state and vel or pos cmd is required, vel state is optional if (!(has_current_position && (has_desired_position || has_desired_velocity))) { return false; } - - if (!has_current_velocity) - { - // First update() after activating does not have velocity available, assume 0 - current_joint_states.velocities.resize(number_of_joints_, 0.0); - } + const std::vector & current_joint_velocities = + has_current_velocity ? current_joint_states.velocities + : std::vector(number_of_joints_, 0.0); // TODO(destogl): please check if we get too much malloc from this initialization, // if so then we should use members instead local variables and initialize them in other method @@ -140,8 +138,7 @@ bool JointSaturationLimiter::on_enfo current_joint_states.positions[index] + desired_vel[index] * dt_seconds; } - desired_acc[index] = - (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds; } } @@ -174,16 +171,15 @@ bool JointSaturationLimiter::on_enfo if ( std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index])) { - desired_acc[index] = - (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds; } // check if decelerating - if velocity is changing toward 0 bool deceleration_limit_applied = false; bool limit_applied = false; if ( - (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) || - (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0)) + (desired_acc[index] < 0 && current_joint_velocities[index] > 0) || + (desired_acc[index] > 0 && current_joint_velocities[index] < 0)) { // limit deceleration if (joint_limits_[index].has_deceleration_limits) @@ -204,13 +200,12 @@ bool JointSaturationLimiter::on_enfo if (limit_applied) { // vel_cmd from integration of desired_acc, needed even if no vel output - desired_vel[index] = - current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds; if (has_desired_position) { // pos_cmd from from double integration of desired_acc desired_pos[index] = current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + + current_joint_velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; } } @@ -315,7 +310,7 @@ bool JointSaturationLimiter::on_enfo // Compute accel to stop // Here we aren't explicitly maximally decelerating, but for joints near their limits this // should still result in max decel being used - desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds; + desired_acc[index] = -current_joint_velocities[index] / dt_seconds; if (joint_limits_[index].has_deceleration_limits) { desired_acc[index] = std::copysign( @@ -332,13 +327,12 @@ bool JointSaturationLimiter::on_enfo // Recompute velocity and position if (has_desired_velocity) { - desired_vel[index] = - current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds; } if (has_desired_position) { desired_pos[index] = current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + + current_joint_velocities[index] * dt_seconds + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; } } diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index 432edb5437..315b236d64 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -19,7 +19,7 @@ namespace joint_limits { bool JointSoftLimiter::on_enforce( - JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const JointControlInterfacesData & actual, JointControlInterfacesData & desired, const rclcpp::Duration & dt) { bool limits_enforced = false; From ea23b7b6fa558400b7cf79c9e98968dfe4ec40ef Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 Jan 2025 22:36:02 +0100 Subject: [PATCH 3/5] Remove debug logs --- joint_limits/src/joint_limits_helpers.cpp | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index ac02ca7de9..61a5e5b313 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -54,19 +54,6 @@ PositionLimits compute_position_limits( act_pos.has_value() ? act_pos.value() : prev_command_pos.value(); pos_limits.lower_limit = std::max(position_reference - delta_pos, pos_limits.lower_limit); pos_limits.upper_limit = std::min(position_reference + delta_pos, pos_limits.upper_limit); - RCLCPP_ERROR_EXPRESSION( - rclcpp::get_logger("joint_limiter_interface"), - act_pos.has_value(), "Joint position limits computed based on the actual position : [%.5f, " - "%.5f].", position_reference, prev_command_pos.value()); - RCLCPP_ERROR_EXPRESSION( - rclcpp::get_logger("joint_limiter_interface"), - !act_pos.has_value(), "Didn't use the actual position to compute the limits.: [%.5f, " - "%.5f].", position_reference, prev_command_pos.value()); - RCLCPP_ERROR_EXPRESSION( - rclcpp::get_logger("joint_limiter_interface"), - act_pos.has_value() || prev_command_pos.has_value(), - "Joint position limits computed based on velocity limits: [%.5f, %.5f].", - pos_limits.lower_limit, pos_limits.upper_limit); } internal::check_and_swap_limits(pos_limits.lower_limit, pos_limits.upper_limit); return pos_limits; From 28ff5e47ad07ad5caeab2d2f632ea4aa0b159b25 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 Jan 2025 22:36:14 +0100 Subject: [PATCH 4/5] Add proper tests for the soft limiter --- joint_limits/test/test_joint_soft_limiter.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp index 7deb2796ea..49aabcc373 100644 --- a/joint_limits/test/test_joint_soft_limiter.cpp +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -162,6 +162,7 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) soft_limits.max_position = 1.5; soft_limits.min_position = -1.5; ASSERT_TRUE(Init(limits, soft_limits)); + actual_state_.position = 0.0; desired_state_.position = 2.0; ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); @@ -170,6 +171,7 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) soft_limits.min_position = -0.75; ASSERT_TRUE(Init(limits, soft_limits)); desired_state_.position = 2.0; + actual_state_ = {}; ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); EXPECT_NEAR(desired_state_.position.value(), soft_limits.max_position, COMMON_THRESHOLD); desired_state_.position = -3.0; @@ -1021,9 +1023,13 @@ TEST_F(JointSoftLimiterTest, check_all_desired_references_limiting) // Desired position and velocity affected due to the acceleration limits test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); - test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); - test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); - test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true); + // If the actual position doesn't change, the desired position should not change + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.2, 0.0, 6.0, 0.0, 0.0, 0.0, 1.7, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.5, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.5, 0.0, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.5, 0.5, 0.5, 0.5, true); test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); From bf41ee1fdf69c0073e095fb6ddc28bc2d21d9119 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 Jan 2025 23:09:17 +0100 Subject: [PATCH 5/5] Add tests resembling a slow system --- joint_limits/test/test_joint_soft_limiter.cpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp index 49aabcc373..3659be4048 100644 --- a/joint_limits/test/test_joint_soft_limiter.cpp +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -288,6 +288,32 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) actual_state_.position = expected_pos; } + // More generic test case to mock a slow system + soft_limits.k_position = 0.5; + soft_limits.max_position = 2.0; + soft_limits.min_position = -2.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + actual_state_.position = -0.1; + for (auto i = 0; i < 10000; i++) + { + SCOPED_TRACE( + "Testing for iteration: " + std::to_string(i) + + " for desired position: " + std::to_string(desired_state_.position.value()) + + " and actual position : " + std::to_string(actual_state_.position.value()) + + " for the joint limits : " + limits.to_string()); + desired_state_.position = 4.0; + const double delta_pos = std::min( + (soft_limits.max_position - actual_state_.position.value()) * soft_limits.k_position, + limits.max_velocity * period.seconds()); + const double expected_pos = actual_state_.position.value() + delta_pos; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_pos, COMMON_THRESHOLD); + actual_state_.position = expected_pos / 1.27; + } + // Now test when there are no position limits and soft limits, then the desired position is not // saturated limits = joint_limits::JointLimits();