Skip to content

Commit ecc5a39

Browse files
mergify[bot]mergify[bot]
authored
Fix disable velocity and effort limiting feature (#3425) (#3437)
1 parent 4d2309b commit ecc5a39

5 files changed

Lines changed: 312 additions & 10 deletions

File tree

‎joint_limits/include/joint_limits/joint_limits_helpers.hpp‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -73,19 +73,22 @@ PositionLimits compute_position_limits(
7373
* @param prev_command_vel The previous commanded velocity of the joint.
7474
* @param dt The time step.
7575
* @return The velocity limits, first is the lower limit and second is the upper limit.
76+
* @note When the velocity limits are disabled, this method returns [-inf, inf].
7677
*/
7778
VelocityLimits compute_velocity_limits(
7879
const std::string & joint_name, const joint_limits::JointLimits & limits,
7980
const double & desired_vel, const std::optional<double> & act_pos,
8081
const std::optional<double> & prev_command_vel, double dt);
8182

8283
/**
83-
* @brief Compute the effort limits based on the position and velocity limits.
84+
* @brief Computes the effort limits based on the position and velocity limits.
85+
*
8486
* @param limits The joint limits.
8587
* @param act_pos The actual position of the joint.
8688
* @param act_vel The actual velocity of the joint.
87-
* @param dt The time step.
88-
* @return The effort limits, first is the lower limit and second is the upper limit.
89+
* @param dt The time step (currently unused).
90+
* @return The effort limits: lower_limit is the minimum allowed effort, upper_limit is the maximum.
91+
* @note When the effort limits are disabled, this method returns [-inf, inf].
8992
*/
9093
EffortLimits compute_effort_limits(
9194
const joint_limits::JointLimits & limits, const std::optional<double> & act_pos,

‎joint_limits/src/joint_limits_helpers.cpp‎

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -140,9 +140,12 @@ VelocityLimits compute_velocity_limits(
140140
const double & desired_vel, const std::optional<double> & act_pos,
141141
const std::optional<double> & prev_command_vel, double dt)
142142
{
143-
const double max_vel =
144-
limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits<double>::infinity();
145-
VelocityLimits vel_limits(-max_vel, max_vel);
143+
if (!limits.has_velocity_limits)
144+
{
145+
return VelocityLimits(
146+
-std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
147+
}
148+
VelocityLimits vel_limits(-limits.max_velocity, limits.max_velocity);
146149
if (limits.has_position_limits && act_pos.has_value())
147150
{
148151
const double actual_pos = act_pos.value();
@@ -197,9 +200,13 @@ EffortLimits compute_effort_limits(
197200
const joint_limits::JointLimits & limits, const std::optional<double> & act_pos,
198201
const std::optional<double> & act_vel, double /*dt*/)
199202
{
200-
const double max_effort =
201-
limits.has_effort_limits ? limits.max_effort : std::numeric_limits<double>::infinity();
202-
EffortLimits eff_limits(-max_effort, max_effort);
203+
// When effort limits are disabled the effort command must pass through untouched
204+
if (!limits.has_effort_limits)
205+
{
206+
return EffortLimits(
207+
-std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
208+
}
209+
EffortLimits eff_limits(-limits.max_effort, limits.max_effort);
203210
if (limits.has_position_limits && act_pos.has_value() && act_vel.has_value())
204211
{
205212
if ((act_pos.value() <= limits.min_position) && (act_vel.value() <= 0.0))

‎joint_limits/src/joint_soft_limiter.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -210,7 +210,9 @@ bool JointSoftLimiter::on_enforce(
210210
joint_name, hard_limits, desired.velocity.value(), actual.position, prev_command_.velocity,
211211
dt_seconds);
212212

213-
if (hard_limits.has_acceleration_limits && actual.has_velocity())
213+
if (
214+
hard_limits.has_velocity_limits && hard_limits.has_acceleration_limits &&
215+
actual.has_velocity())
214216
{
215217
soft_min_vel =
216218
std::max(actual.velocity.value() - hard_limits.max_acceleration * dt_seconds, soft_min_vel);

‎joint_limits/test/test_joint_range_limiter.cpp‎

Lines changed: 134 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -337,6 +337,61 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases)
337337
test_limit_enforcing(outside_limits_pos, -1.0, 0.0, true);
338338
}
339339

340+
TEST_F(JointSaturationLimiterTest, when_velocity_limits_disabled_expect_no_velocity_enforcement)
341+
{
342+
SetupNode("joint_saturation_limiter");
343+
ASSERT_TRUE(Load());
344+
345+
// velocity limits are disabled, but position and acceleration limits are still active
346+
joint_limits::JointLimits limits;
347+
limits.has_position_limits = true;
348+
limits.min_position = -5.0;
349+
limits.max_position = 5.0;
350+
limits.has_acceleration_limits = true;
351+
limits.max_acceleration = 0.5;
352+
limits.has_velocity_limits = false;
353+
ASSERT_TRUE(Init(limits));
354+
ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
355+
356+
rclcpp::Duration period(1, 0);
357+
358+
auto test_velocity_passes_through =
359+
[&](const std::optional<double> & actual_position, double desired_velocity)
360+
{
361+
desired_state_ = {};
362+
actual_state_ = {};
363+
const double act_pos = actual_position.has_value() ? actual_position.value()
364+
: std::numeric_limits<double>::quiet_NaN();
365+
SCOPED_TRACE(
366+
"Testing velocity passthrough for actual position: " + std::to_string(act_pos) +
367+
", desired velocity: " + std::to_string(desired_velocity) +
368+
" for the joint limits : " + limits.to_string());
369+
if (actual_position.has_value())
370+
{
371+
actual_state_.position = actual_position.value();
372+
}
373+
desired_state_.velocity = desired_velocity;
374+
ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
375+
EXPECT_TRUE(desired_state_.has_velocity());
376+
EXPECT_NEAR(desired_state_.velocity.value(), desired_velocity, COMMON_THRESHOLD);
377+
};
378+
379+
// Normal operating region: any velocity must pass through unchanged
380+
test_velocity_passes_through(0.0, 1000.0);
381+
test_velocity_passes_through(0.0, -1000.0);
382+
383+
test_velocity_passes_through(5.0, 1000.0);
384+
test_velocity_passes_through(5.0, -1000.0);
385+
test_velocity_passes_through(6.0, 1000.0);
386+
test_velocity_passes_through(-5.0, -1000.0);
387+
test_velocity_passes_through(-5.0, 1000.0);
388+
test_velocity_passes_through(-6.0, -1000.0);
389+
390+
test_velocity_passes_through(0.0, 1000.0);
391+
test_velocity_passes_through(0.0, -1000.0);
392+
test_velocity_passes_through(0.0, 1000.0);
393+
}
394+
340395
TEST_F(JointSaturationLimiterTest, check_desired_effort_only_cases)
341396
{
342397
SetupNode("joint_saturation_limiter");
@@ -452,6 +507,85 @@ TEST_F(JointSaturationLimiterTest, check_desired_effort_only_cases)
452507
test_limit_enforcing(-5.0, -0.2, 30.0, 30.0, false);
453508
}
454509

510+
TEST_F(JointSaturationLimiterTest, when_effort_limits_disabled_expect_no_effort_enforcement)
511+
{
512+
SetupNode("joint_saturation_limiter");
513+
ASSERT_TRUE(Load());
514+
515+
// effort limits are OFF, but position and velocity limits are still active
516+
joint_limits::JointLimits limits;
517+
limits.has_position_limits = true;
518+
limits.min_position = -5.0;
519+
limits.max_position = 5.0;
520+
limits.has_velocity_limits = true;
521+
limits.max_velocity = 1.0;
522+
limits.has_effort_limits = false; // <-- disabled
523+
ASSERT_TRUE(Init(limits));
524+
ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
525+
526+
rclcpp::Duration period(1, 0); // 1 second
527+
528+
auto test_effort_passes_through = [&](
529+
const std::optional<double> & actual_position,
530+
const std::optional<double> & actual_velocity,
531+
double desired_effort)
532+
{
533+
desired_state_ = {};
534+
actual_state_ = {};
535+
const double act_pos = actual_position.has_value() ? actual_position.value()
536+
: std::numeric_limits<double>::quiet_NaN();
537+
const double act_vel = actual_velocity.has_value() ? actual_velocity.value()
538+
: std::numeric_limits<double>::quiet_NaN();
539+
SCOPED_TRACE(
540+
"Testing effort passthrough for actual position: " + std::to_string(act_pos) +
541+
", actual velocity: " + std::to_string(act_vel) + ", desired effort: " +
542+
std::to_string(desired_effort) + " for the joint limits : " + limits.to_string());
543+
if (actual_position.has_value())
544+
{
545+
actual_state_.position = actual_position.value();
546+
}
547+
if (actual_velocity.has_value())
548+
{
549+
actual_state_.velocity = actual_velocity.value();
550+
}
551+
desired_state_.effort = desired_effort;
552+
// With effort limits disabled, enforce() must NOT clamp effort regardless of
553+
// the position/velocity state it must pass through completely untouched.
554+
ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
555+
EXPECT_TRUE(desired_state_.has_effort());
556+
EXPECT_NEAR(desired_state_.effort.value(), desired_effort, COMMON_THRESHOLD);
557+
};
558+
559+
// Normal operating region: any effort must pass through unchanged
560+
test_effort_passes_through(0.0, 0.0, 10000.0);
561+
test_effort_passes_through(0.0, 0.0, -10000.0);
562+
563+
// At max position + zero velocity (old code zeroed upper_limit to 0.0 here)
564+
test_effort_passes_through(5.0, 0.0, 10000.0);
565+
test_effort_passes_through(5.0, 0.0, -10000.0);
566+
// Beyond max position
567+
test_effort_passes_through(6.0, 0.0, 10000.0);
568+
test_effort_passes_through(6.0, 0.0, -10000.0);
569+
570+
// At max position + positive velocity (moving further out)
571+
test_effort_passes_through(5.0, 0.2, 10000.0);
572+
test_effort_passes_through(5.0, 0.2, -10000.0);
573+
574+
// At min position + zero velocity (old code zeroed lower_limit to 0.0 here)
575+
test_effort_passes_through(-5.0, 0.0, 10000.0);
576+
test_effort_passes_through(-5.0, 0.0, -10000.0);
577+
// At min position + negative velocity (moving further out)
578+
test_effort_passes_through(-5.0, -0.2, 10000.0);
579+
test_effort_passes_through(-5.0, -0.2, -10000.0);
580+
581+
// Over max velocity (old code zeroed upper_limit to 0.0 here)
582+
test_effort_passes_through(0.0, 1.5, 10000.0);
583+
test_effort_passes_through(0.0, 1.5, -10000.0);
584+
// Over negative max velocity
585+
test_effort_passes_through(0.0, -1.5, 10000.0);
586+
test_effort_passes_through(0.0, -1.5, -10000.0);
587+
}
588+
455589
TEST_F(JointSaturationLimiterTest, check_desired_acceleration_only_cases)
456590
{
457591
SetupNode("joint_saturation_limiter");

‎joint_limits/test/test_joint_soft_limiter.cpp‎

Lines changed: 156 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -593,6 +593,71 @@ TEST_F(JointSoftLimiterTest, check_desired_velocity_only_cases)
593593
test_limit_enforcing(outside_limits_pos, -1.0, 0.0, true);
594594
}
595595

596+
TEST_F(JointSoftLimiterTest, when_velocity_limits_disabled_expect_no_velocity_enforcement)
597+
{
598+
SetupNode("joint_saturation_limiter");
599+
ASSERT_TRUE(Load());
600+
601+
// velocity limits are disabled, but position and acceleration limits are still active
602+
joint_limits::JointLimits limits;
603+
limits.has_position_limits = true;
604+
limits.min_position = -5.0;
605+
limits.max_position = 5.0;
606+
limits.has_acceleration_limits = true;
607+
limits.max_acceleration = 0.5;
608+
limits.has_velocity_limits = false;
609+
joint_limits::SoftJointLimits soft_limits;
610+
ASSERT_TRUE(Init(limits, soft_limits));
611+
last_commanded_state_ = {};
612+
ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
613+
614+
rclcpp::Duration period(1, 0);
615+
616+
auto test_velocity_passes_through = [&](
617+
const std::optional<double> & actual_position,
618+
const std::optional<double> & actual_velocity,
619+
double desired_velocity)
620+
{
621+
desired_state_ = {};
622+
actual_state_ = {};
623+
const double act_pos = actual_position.has_value() ? actual_position.value()
624+
: std::numeric_limits<double>::quiet_NaN();
625+
const double act_vel = actual_velocity.has_value() ? actual_velocity.value()
626+
: std::numeric_limits<double>::quiet_NaN();
627+
SCOPED_TRACE(
628+
"Testing velocity passthrough for actual position: " + std::to_string(act_pos) +
629+
", actual velocity: " + std::to_string(act_vel) + ", desired velocity: " +
630+
std::to_string(desired_velocity) + " for the joint limits : " + limits.to_string());
631+
if (actual_position.has_value())
632+
{
633+
actual_state_.position = actual_position.value();
634+
}
635+
if (actual_velocity.has_value())
636+
{
637+
actual_state_.velocity = actual_velocity.value();
638+
}
639+
desired_state_.velocity = desired_velocity;
640+
ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
641+
EXPECT_TRUE(desired_state_.has_velocity());
642+
EXPECT_NEAR(desired_state_.velocity.value(), desired_velocity, COMMON_THRESHOLD);
643+
};
644+
645+
test_velocity_passes_through(0.0, std::nullopt, 1000.0);
646+
test_velocity_passes_through(5.0, std::nullopt, 1000.0);
647+
test_velocity_passes_through(5.0, std::nullopt, -1000.0);
648+
test_velocity_passes_through(-5.0, std::nullopt, -1000.0);
649+
test_velocity_passes_through(-5.0, std::nullopt, 1000.0);
650+
651+
// Reset prev_command_ so the next block isn't influenced
652+
ASSERT_TRUE(Init(limits, soft_limits));
653+
last_commanded_state_ = {};
654+
ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
655+
656+
test_velocity_passes_through(0.0, 5.0, 1000.0);
657+
test_velocity_passes_through(0.0, -5.0, -1000.0);
658+
test_velocity_passes_through(0.0, 0.0, 1000.0);
659+
}
660+
596661
TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
597662
{
598663
SetupNode("joint_saturation_limiter");
@@ -878,6 +943,97 @@ TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
878943
}
879944
}
880945

946+
TEST_F(JointSoftLimiterTest, when_effort_limits_disabled_expect_no_effort_enforcement)
947+
{
948+
SetupNode("joint_saturation_limiter");
949+
ASSERT_TRUE(Load());
950+
951+
// effort limits are OFF, but position and velocity limits are still active
952+
joint_limits::JointLimits limits;
953+
limits.has_position_limits = true;
954+
limits.min_position = -5.0;
955+
limits.max_position = 5.0;
956+
limits.has_velocity_limits = true;
957+
limits.max_velocity = 1.0;
958+
limits.has_effort_limits = false; // <-- disabled
959+
joint_limits::SoftJointLimits soft_limits;
960+
ASSERT_TRUE(Init(limits, soft_limits));
961+
last_commanded_state_ = {};
962+
ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
963+
964+
rclcpp::Duration period(1, 0); // 1 second
965+
966+
auto test_effort_passes_through = [&](
967+
const std::optional<double> & actual_position,
968+
const std::optional<double> & actual_velocity,
969+
double desired_effort)
970+
{
971+
desired_state_ = {};
972+
actual_state_ = {};
973+
const double act_pos = actual_position.has_value() ? actual_position.value()
974+
: std::numeric_limits<double>::quiet_NaN();
975+
const double act_vel = actual_velocity.has_value() ? actual_velocity.value()
976+
: std::numeric_limits<double>::quiet_NaN();
977+
SCOPED_TRACE(
978+
"Testing effort passthrough for actual position: " + std::to_string(act_pos) +
979+
", actual velocity: " + std::to_string(act_vel) + ", desired effort: " +
980+
std::to_string(desired_effort) + " for the joint limits : " + limits.to_string());
981+
if (actual_position.has_value())
982+
{
983+
actual_state_.position = actual_position.value();
984+
}
985+
if (actual_velocity.has_value())
986+
{
987+
actual_state_.velocity = actual_velocity.value();
988+
}
989+
desired_state_.effort = desired_effort;
990+
// With effort limits disabled, enforce() must NOT clamp effort regardless of
991+
// the position/velocity state it must pass through completely untouched.
992+
ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
993+
EXPECT_TRUE(desired_state_.has_effort());
994+
EXPECT_NEAR(desired_state_.effort.value(), desired_effort, COMMON_THRESHOLD);
995+
};
996+
997+
// Normal operating region: any effort must pass through unchanged
998+
test_effort_passes_through(0.0, 0.0, 10000.0);
999+
test_effort_passes_through(0.0, 0.0, -10000.0);
1000+
1001+
// At max position + zero velocity (old code zeroed upper_limit to 0.0 here)
1002+
test_effort_passes_through(5.0, 0.0, 10000.0);
1003+
test_effort_passes_through(5.0, 0.0, -10000.0);
1004+
// Beyond max position
1005+
test_effort_passes_through(6.0, 0.0, 10000.0);
1006+
test_effort_passes_through(6.0, 0.0, -10000.0);
1007+
1008+
// At max position + positive velocity (moving further out)
1009+
test_effort_passes_through(5.0, 0.2, 10000.0);
1010+
test_effort_passes_through(5.0, 0.2, -10000.0);
1011+
1012+
// At min position + zero velocity (old code zeroed lower_limit to 0.0 here)
1013+
test_effort_passes_through(-5.0, 0.0, 10000.0);
1014+
test_effort_passes_through(-5.0, 0.0, -10000.0);
1015+
// At min position + negative velocity (moving further out)
1016+
test_effort_passes_through(-5.0, -0.2, 10000.0);
1017+
test_effort_passes_through(-5.0, -0.2, -10000.0);
1018+
1019+
// Over max velocity (old code zeroed upper_limit to 0.0 here)
1020+
test_effort_passes_through(0.0, 1.5, 10000.0);
1021+
test_effort_passes_through(0.0, 1.5, -10000.0);
1022+
// Over negative max velocity
1023+
test_effort_passes_through(0.0, -1.5, 10000.0);
1024+
test_effort_passes_through(0.0, -1.5, -10000.0);
1025+
1026+
// Also verify with k_velocity set: soft effort shaping must also be skipped
1027+
soft_limits.k_velocity = 5000.0;
1028+
ASSERT_TRUE(Init(limits, soft_limits));
1029+
last_commanded_state_ = {};
1030+
ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
1031+
test_effort_passes_through(0.0, 0.5, 10000.0);
1032+
test_effort_passes_through(0.0, 0.5, -10000.0);
1033+
test_effort_passes_through(5.0, 0.2, 10000.0);
1034+
test_effort_passes_through(5.0, 0.2, -10000.0);
1035+
}
1036+
8811037
TEST_F(JointSoftLimiterTest, check_desired_acceleration_only_cases)
8821038
{
8831039
SetupNode("joint_saturation_limiter");

0 commit comments

Comments
 (0)