From 1d96350b13688622a22ed2f13ea57c9fb757d03d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 11 Oct 2024 14:27:29 +0200 Subject: [PATCH] fix the soft limiting logic and add respective tests --- joint_limits/src/joint_soft_limiter.cpp | 14 +++--- joint_limits/test/test_joint_soft_limiter.cpp | 49 +++++++++++++++---- 2 files changed, 47 insertions(+), 16 deletions(-) diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index fdac151557..a3d0d81ee9 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -96,13 +96,13 @@ bool JointSoftLimiter::on_enforce( double soft_max_vel = std::numeric_limits::infinity(); double position = std::numeric_limits::infinity(); - if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) + if (actual.has_position()) { - position = prev_command_.position.value(); + position = actual.position.value(); } - else if (actual.has_position()) + else if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) { - position = actual.position.value(); + position = prev_command_.position.value(); } if (hard_limits.has_velocity_limits) @@ -122,7 +122,9 @@ bool JointSoftLimiter::on_enforce( -soft_joint_limits.k_position * (position - soft_joint_limits.max_position), -hard_limits.max_velocity, hard_limits.max_velocity); - if ((position < hard_limits.min_position) || (position > hard_limits.max_position)) + if ( + (position < (hard_limits.min_position - internal::POSITION_BOUNDS_TOLERANCE)) || + (position > (hard_limits.max_position + internal::POSITION_BOUNDS_TOLERANCE))) { soft_min_vel = 0.0; soft_max_vel = 0.0; @@ -130,7 +132,7 @@ bool JointSoftLimiter::on_enforce( else if ( (position < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) { - constexpr double soft_limit_reach_velocity = 1.0 * (M_PI / 180.0); + const double soft_limit_reach_velocity = 1.0 * (M_PI / 180.0); soft_min_vel = std::copysign(soft_limit_reach_velocity, soft_min_vel); soft_max_vel = std::copysign(soft_limit_reach_velocity, soft_max_vel); } diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp index d0d814dff9..7deb2796ea 100644 --- a/joint_limits/test/test_joint_soft_limiter.cpp +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -214,6 +214,11 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) 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.95; + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.95, COMMON_THRESHOLD); + actual_state_.position = 1.5; desired_state_.position = 2.0; ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); EXPECT_NEAR(desired_state_.position.value(), 2.0, COMMON_THRESHOLD); @@ -228,21 +233,40 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) 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.45; + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.45, COMMON_THRESHOLD); + actual_state_.position = 0.95; desired_state_.position = 2.0; ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); EXPECT_NEAR(desired_state_.position.value(), 1.5, COMMON_THRESHOLD); - soft_limits.k_position = 2.0; + // The case of no actual position feedback + soft_limits.k_position = 0.5; soft_limits.max_position = 1.5; soft_limits.min_position = -1.5; 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 = std::nullopt; + desired_state_.position = 0.2; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.2, COMMON_THRESHOLD); + auto prev_command_state = desired_state_; + while (desired_state_.position.value() < (soft_limits.max_position - COMMON_THRESHOLD)) + { + desired_state_.position = 2.0; + double expected_pos = + prev_command_state.position.value() + + (soft_limits.max_position - prev_command_state.position.value()) * soft_limits.k_position; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_pos, COMMON_THRESHOLD); + prev_command_state = desired_state_; + } desired_state_.position = 2.0; ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); EXPECT_NEAR(desired_state_.position.value(), 1.5, COMMON_THRESHOLD); + // More generic test case soft_limits.k_position = 0.5; soft_limits.max_position = 2.0; soft_limits.min_position = -2.0; @@ -250,12 +274,17 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) 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); - desired_state_.position = 2.0; - ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); - EXPECT_NEAR(desired_state_.position.value(), 1.5, COMMON_THRESHOLD); - desired_state_.position = 2.0; - ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); - EXPECT_NEAR(desired_state_.position.value(), 1.75, COMMON_THRESHOLD); + actual_state_.position = 0.2; + while (actual_state_.position.value() < (desired_state_.position.value() - COMMON_THRESHOLD)) + { + desired_state_.position = 2.0; + double expected_pos = + actual_state_.position.value() + + (soft_limits.max_position - actual_state_.position.value()) * soft_limits.k_position; + 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; + } // Now test when there are no position limits and soft limits, then the desired position is not // saturated