Skip to content

Commit a60d00a

Browse files
saikishorsaikishor
authored
Handle NaNs properly in the joint limiters (#3320)
1 parent b95b46d commit a60d00a

5 files changed

Lines changed: 250 additions & 48 deletions

File tree

‎joint_limits/src/joint_limits_helpers.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -75,23 +75,23 @@ void verify_actual_position_within_limits(
7575
void update_prev_command(
7676
const JointControlInterfacesData & desired, JointControlInterfacesData & prev_command)
7777
{
78-
if (desired.has_position())
78+
if (desired.has_position() && !std::isnan(desired.position.value()))
7979
{
8080
prev_command.position = desired.position;
8181
}
82-
if (desired.has_velocity())
82+
if (desired.has_velocity() && !std::isnan(desired.velocity.value()))
8383
{
8484
prev_command.velocity = desired.velocity;
8585
}
86-
if (desired.has_effort())
86+
if (desired.has_effort() && !std::isnan(desired.effort.value()))
8787
{
8888
prev_command.effort = desired.effort;
8989
}
90-
if (desired.has_acceleration())
90+
if (desired.has_acceleration() && !std::isnan(desired.acceleration.value()))
9191
{
9292
prev_command.acceleration = desired.acceleration;
9393
}
94-
if (desired.has_jerk())
94+
if (desired.has_jerk() && !std::isnan(desired.jerk.value()))
9595
{
9696
prev_command.jerk = desired.jerk;
9797
}

‎joint_limits/src/joint_range_limiter.cpp‎

Lines changed: 45 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -63,24 +63,58 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
6363
{
6464
if (desired.has_position())
6565
{
66-
prev_command_.position = actual.has_position() ? actual.position : desired.position;
66+
if (actual.has_position())
67+
{
68+
prev_command_.position = actual.position;
69+
}
70+
else if (!std::isnan(desired.position.value()))
71+
{
72+
prev_command_.position = desired.position;
73+
}
6774
}
6875
if (desired.has_velocity())
6976
{
70-
prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity;
77+
if (actual.has_velocity())
78+
{
79+
prev_command_.velocity = actual.velocity;
80+
}
81+
else if (!std::isnan(desired.velocity.value()))
82+
{
83+
prev_command_.velocity = desired.velocity;
84+
}
7185
}
7286
if (desired.has_effort())
7387
{
74-
prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort;
88+
if (actual.has_effort())
89+
{
90+
prev_command_.effort = actual.effort;
91+
}
92+
else if (!std::isnan(desired.effort.value()))
93+
{
94+
prev_command_.effort = desired.effort;
95+
}
7596
}
7697
if (desired.has_acceleration())
7798
{
78-
prev_command_.acceleration =
79-
actual.has_acceleration() ? actual.acceleration : desired.acceleration;
99+
if (actual.has_acceleration())
100+
{
101+
prev_command_.acceleration = actual.acceleration;
102+
}
103+
else if (!std::isnan(desired.acceleration.value()))
104+
{
105+
prev_command_.acceleration = desired.acceleration;
106+
}
80107
}
81108
if (desired.has_jerk())
82109
{
83-
prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk;
110+
if (actual.has_jerk())
111+
{
112+
prev_command_.jerk = actual.jerk;
113+
}
114+
else if (!std::isnan(desired.jerk.value()))
115+
{
116+
prev_command_.jerk = desired.jerk;
117+
}
84118
}
85119
if (actual.has_data())
86120
{
@@ -92,7 +126,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
92126
}
93127
}
94128

95-
if (desired.has_position())
129+
if (desired.has_position() && !std::isnan(desired.position.value()))
96130
{
97131
const auto limits = compute_position_limits(
98132
joint_name, joint_limits, actual.velocity, actual.position, prev_command_.position,
@@ -101,7 +135,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
101135
desired.position = std::clamp(desired.position.value(), limits.lower_limit, limits.upper_limit);
102136
}
103137

104-
if (desired.has_velocity())
138+
if (desired.has_velocity() && !std::isnan(desired.velocity.value()))
105139
{
106140
const auto limits = compute_velocity_limits(
107141
joint_name, joint_limits, desired.velocity.value(), actual.position, prev_command_.velocity,
@@ -112,7 +146,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
112146
desired.velocity = std::clamp(desired.velocity.value(), limits.lower_limit, limits.upper_limit);
113147
}
114148

115-
if (desired.has_effort())
149+
if (desired.has_effort() && !std::isnan(desired.effort.value()))
116150
{
117151
const auto limits =
118152
compute_effort_limits(joint_limits, actual.position, actual.velocity, dt_seconds);
@@ -121,7 +155,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
121155
desired.effort = std::clamp(desired.effort.value(), limits.lower_limit, limits.upper_limit);
122156
}
123157

124-
if (desired.has_acceleration())
158+
if (desired.has_acceleration() && !std::isnan(desired.acceleration.value()))
125159
{
126160
const auto limits =
127161
compute_acceleration_limits(joint_limits, desired.acceleration.value(), actual.velocity);
@@ -132,7 +166,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
132166
std::clamp(desired.acceleration.value(), limits.lower_limit, limits.upper_limit);
133167
}
134168

135-
if (desired.has_jerk())
169+
if (desired.has_jerk() && !std::isnan(desired.jerk.value()))
136170
{
137171
limits_enforced =
138172
is_limited(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk) ||

‎joint_limits/src/joint_soft_limiter.cpp‎

Lines changed: 45 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -49,24 +49,58 @@ bool JointSoftLimiter::on_enforce(
4949
{
5050
if (desired.has_position())
5151
{
52-
prev_command_.position = actual.has_position() ? actual.position : desired.position;
52+
if (actual.has_position())
53+
{
54+
prev_command_.position = actual.position;
55+
}
56+
else if (!std::isnan(desired.position.value()))
57+
{
58+
prev_command_.position = desired.position;
59+
}
5360
}
5461
if (desired.has_velocity())
5562
{
56-
prev_command_.velocity = actual.has_velocity() ? actual.velocity : desired.velocity;
63+
if (actual.has_velocity())
64+
{
65+
prev_command_.velocity = actual.velocity;
66+
}
67+
else if (!std::isnan(desired.velocity.value()))
68+
{
69+
prev_command_.velocity = desired.velocity;
70+
}
5771
}
5872
if (desired.has_effort())
5973
{
60-
prev_command_.effort = actual.has_effort() ? actual.effort : desired.effort;
74+
if (actual.has_effort())
75+
{
76+
prev_command_.effort = actual.effort;
77+
}
78+
else if (!std::isnan(desired.effort.value()))
79+
{
80+
prev_command_.effort = desired.effort;
81+
}
6182
}
6283
if (desired.has_acceleration())
6384
{
64-
prev_command_.acceleration =
65-
actual.has_acceleration() ? actual.acceleration : desired.acceleration;
85+
if (actual.has_acceleration())
86+
{
87+
prev_command_.acceleration = actual.acceleration;
88+
}
89+
else if (!std::isnan(desired.acceleration.value()))
90+
{
91+
prev_command_.acceleration = desired.acceleration;
92+
}
6693
}
6794
if (desired.has_jerk())
6895
{
69-
prev_command_.jerk = actual.has_jerk() ? actual.jerk : desired.jerk;
96+
if (actual.has_jerk())
97+
{
98+
prev_command_.jerk = actual.jerk;
99+
}
100+
else if (!std::isnan(desired.jerk.value()))
101+
{
102+
prev_command_.jerk = desired.jerk;
103+
}
70104
}
71105
if (actual.has_data())
72106
{
@@ -132,7 +166,7 @@ bool JointSoftLimiter::on_enforce(
132166
}
133167
}
134168

135-
if (desired.has_position())
169+
if (desired.has_position() && !std::isnan(desired.position.value()))
136170
{
137171
const auto position_limits = compute_position_limits(
138172
joint_name, hard_limits, actual.velocity, actual.position, prev_command_.position,
@@ -170,7 +204,7 @@ bool JointSoftLimiter::on_enforce(
170204
desired.position = std::clamp(desired.position.value(), pos_low, pos_high);
171205
}
172206

173-
if (desired.has_velocity())
207+
if (desired.has_velocity() && !std::isnan(desired.velocity.value()))
174208
{
175209
const auto velocity_limits = compute_velocity_limits(
176210
joint_name, hard_limits, desired.velocity.value(), actual.position, prev_command_.velocity,
@@ -192,7 +226,7 @@ bool JointSoftLimiter::on_enforce(
192226
desired.velocity = std::clamp(desired.velocity.value(), soft_min_vel, soft_max_vel);
193227
}
194228

195-
if (desired.has_effort())
229+
if (desired.has_effort() && !std::isnan(desired.effort.value()))
196230
{
197231
const auto effort_limits =
198232
compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds);
@@ -221,7 +255,7 @@ bool JointSoftLimiter::on_enforce(
221255
desired.effort = std::clamp(desired.effort.value(), soft_min_eff, soft_max_eff);
222256
}
223257

224-
if (desired.has_acceleration())
258+
if (desired.has_acceleration() && !std::isnan(desired.acceleration.value()))
225259
{
226260
const auto limits =
227261
compute_acceleration_limits(hard_limits, desired.acceleration.value(), actual.velocity);
@@ -232,35 +266,14 @@ bool JointSoftLimiter::on_enforce(
232266
std::clamp(desired.acceleration.value(), limits.lower_limit, limits.upper_limit);
233267
}
234268

235-
if (desired.has_jerk())
269+
if (desired.has_jerk() && !std::isnan(desired.jerk.value()))
236270
{
237271
limits_enforced =
238272
is_limited(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk) ||
239273
limits_enforced;
240274
desired.jerk = std::clamp(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk);
241275
}
242276

243-
if (desired.has_position() && !std::isfinite(desired.position.value()) && actual.has_position())
244-
{
245-
desired.position = actual.position;
246-
limits_enforced = true;
247-
}
248-
if (desired.has_velocity() && !std::isfinite(desired.velocity.value()))
249-
{
250-
desired.velocity = 0.0;
251-
limits_enforced = true;
252-
}
253-
if (desired.has_acceleration() && !std::isfinite(desired.acceleration.value()))
254-
{
255-
desired.acceleration = 0.0;
256-
limits_enforced = true;
257-
}
258-
if (desired.has_jerk() && !std::isfinite(desired.jerk.value()))
259-
{
260-
desired.jerk = 0.0;
261-
limits_enforced = true;
262-
}
263-
264277
update_prev_command(desired, prev_command_);
265278

266279
return limits_enforced;

‎joint_limits/test/test_joint_range_limiter.cpp‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -672,6 +672,83 @@ TEST_F(JointSaturationLimiterTest, check_all_desired_references_limiting)
672672
test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true);
673673
}
674674

675+
TEST_F(JointSaturationLimiterTest, when_command_is_nan_expect_no_limiting)
676+
{
677+
SetupNode("joint_saturation_limiter");
678+
ASSERT_TRUE(Load());
679+
680+
joint_limits::JointLimits limits;
681+
limits.has_position_limits = true;
682+
limits.min_position = -M_PI;
683+
limits.max_position = M_PI;
684+
limits.has_velocity_limits = true;
685+
limits.max_velocity = 1.0;
686+
limits.has_acceleration_limits = true;
687+
limits.max_acceleration = 0.5;
688+
limits.has_effort_limits = true;
689+
limits.max_effort = 200.0;
690+
limits.has_jerk_limits = true;
691+
limits.max_jerk = 2.0;
692+
ASSERT_TRUE(Init(limits));
693+
ASSERT_TRUE(Configure());
694+
695+
rclcpp::Duration period(1, 0); // 1 second
696+
const double nan = std::numeric_limits<double>::quiet_NaN();
697+
698+
// NaN position must pass through unchanged
699+
desired_state_ = {};
700+
actual_state_ = {};
701+
desired_state_.position = nan;
702+
EXPECT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
703+
EXPECT_TRUE(std::isnan(desired_state_.position.value()));
704+
705+
// NaN velocity must pass through unchanged
706+
desired_state_ = {};
707+
actual_state_ = {};
708+
desired_state_.velocity = nan;
709+
EXPECT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
710+
EXPECT_TRUE(std::isnan(desired_state_.velocity.value()));
711+
712+
// NaN effort must pass through unchanged
713+
desired_state_ = {};
714+
actual_state_ = {};
715+
desired_state_.effort = nan;
716+
EXPECT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
717+
EXPECT_TRUE(std::isnan(desired_state_.effort.value()));
718+
719+
// NaN acceleration must pass through unchanged
720+
desired_state_ = {};
721+
actual_state_ = {};
722+
desired_state_.acceleration = nan;
723+
EXPECT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
724+
EXPECT_TRUE(std::isnan(desired_state_.acceleration.value()));
725+
726+
// NaN jerk must pass through unchanged
727+
desired_state_ = {};
728+
actual_state_ = {};
729+
desired_state_.jerk = nan;
730+
EXPECT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
731+
EXPECT_TRUE(std::isnan(desired_state_.jerk.value()));
732+
733+
// NaN must not corrupt prev_command_ — a subsequent finite command must still be limited.
734+
ASSERT_TRUE(Init(limits));
735+
actual_state_ = {};
736+
actual_state_.position = 0.0;
737+
actual_state_.velocity = 0.0;
738+
desired_state_ = {};
739+
desired_state_.position = 0.0;
740+
ASSERT_FALSE(
741+
joint_limiter_->enforce(actual_state_, desired_state_, period)); // seed prev_command
742+
743+
desired_state_.position = nan;
744+
EXPECT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); // NaN, no change
745+
746+
desired_state_.position = M_PI * 2.0; // well outside position limits
747+
EXPECT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
748+
EXPECT_TRUE(std::isfinite(desired_state_.position.value()));
749+
EXPECT_LE(desired_state_.position.value(), limits.max_position);
750+
}
751+
675752
int main(int argc, char ** argv)
676753
{
677754
::testing::InitGoogleMock(&argc, argv);

0 commit comments

Comments
 (0)