Warn user when implicit vel exceeds max vel and recal effective acc limits by sachinkum0009 · Pull Request #3456 · ros-controls/ros2_control

sachinkum0009 · 2026-07-10T12:08:05Z

Description

Warn when dt is too large for acceleration limits and implicit vel exceeds max velocity

Add code to warn users when implicit vel exceeds the max vel
Add code to recal effective max acceleration/deceleration.

Fixes #Issue 2434

Is this user-facing behavior change?

Not much only the users will get warn when implicit vel execeeds max vel

Did you use Generative AI?

No

Additional Information

TODOs

To send us a pull request, please:

Fork the repository.
Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change.
Ensure local tests pass. (colcon test and pre-commit run (requires you to install pre-commit by pip3 install pre-commit)
Commit to your fork using clear commit messages.
Send a pull request, answering any default questions in the pull request interface.
Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.

Warn when dt is too large for acceleration limits and clamp desired velocity to satisfy max velocity constraints. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

The velocity clipping is handled later in the limiter loop, making this redundant calculation unnecessary. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

codecov · 2026-07-10T13:52:21Z

Codecov Report

❌ Patch coverage is 93.75000% with 2 lines in your changes missing coverage. Please review.
✅ Project coverage is 89.52%. Comparing base (427cb38) to head (4c01a70).
⚠️ Report is 1 commits behind head on master.

Files with missing lines	Patch %	Lines
joint_limits/src/joint_saturation_limiter.cpp	93.75%	0 Missing and 1 partial ⚠️
...oint_limits/test/test_joint_saturation_limiter.cpp	91.66%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@           Coverage Diff           @@
##           master    #3456   +/-   ##
=======================================
  Coverage   89.51%   89.52%           
=======================================
  Files         164      164           
  Lines       21269    21295   +26     
  Branches     1656     1662    +6     
=======================================
+ Hits        19040    19064   +24     
  Misses       1529     1529           
- Partials      700      702    +2

Flag	Coverage Δ
unittests	`89.52% <93.75%> (+<0.01%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
.../include/joint_limits/joint_saturation_limiter.hpp	`100.00% <100.00%> (ø)`
joint_limits/src/joint_saturation_limiter.cpp	`85.63% <93.75%> (+0.25%)`	⬆️
...oint_limits/test/test_joint_saturation_limiter.cpp	`91.21% <91.66%> (-0.02%)`	⬇️

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Caches effective acceleration and deceleration limits to ensure they do not exceed the velocity limit when the control cycle duration (dt) changes. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

sachinkum0009 · 2026-07-10T14:50:14Z

    desired_joint_states_.positions[0] = 1.0;
    desired_joint_states_.velocities[0] = 1.5;     // valid pos derivative as well
-    desired_joint_states_.accelerations[0] = 2.9;  // valid pos derivative as well
+    desired_joint_states_.accelerations[0] = 2.0;  // valid pos derivative as well


Hi @destogl, when recalculating the max acc/dec if impl vel exceed max vel, this test fails. So I have updated the test. I want to ask if it is fine or should I do something else?

Pleaes create a new test for this and call it "when_derivative_too_high_expect_lower_effective_limits".

Also make the exact values checks.

effective acc. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

destogl

Generally good, few smaller issues in tests.

destogl · 2026-07-13T14:56:08Z

    prev_command_ = current_joint_states;
+    const auto num_joints = this->number_of_joints_;
+    prev_dt_seconds_ = 0.0;
+    effective_max_acc_.assign(num_joints, 0.0);


do we really need here additional variables? Why? If this is just calculated, we don't need to keep the values. And if we are just storing them locally within a method, this should be on a heap and should not initialize any memory.

This makes sense as we need to later adjust all joint togeather.

Thanks, I decided to remove the dt recalculation logic as in RT system dt should be constant. Now will remove these additional variables and update the on_enforce to update the max acc/dec when implicit vel exceeds the max vel.

Pls LMK if is approach is good?

destogl · 2026-07-13T14:56:52Z

-  // TODO(gwalck) compute if the max are not implicitly violated with the given dt
-  // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4
-  // velocity max is implicitly already violated due to max_acc * dt > 2.0
+  // if the dt is changed then recalculate the max_acceleration/max_deceleration if implicit vel


what does it mean that dt is changend?

Isn't this also a problem for the velocity too?

destogl · 2026-07-13T14:57:53Z

+        }
+        else
+        {
+          effective_max_acc_[i] = joint_limits_[i].max_acceleration;


this might cause a problem if done only for one joint. We should scale down then all other joints appropriately...

If we are for now not scaling down also in other cases, then we don't have to do now, but in the followup PR, as this is important to not change robot trajectories.

Thanks, I am thinking of calculating the max_reduction_factor = max_vel/implicit_vel, for all the joints and then use the min max_reduction_factor to scale down all the joints acc/dec.

Pls LMK if this approach is good?

destogl · 2026-07-13T17:34:38Z

    desired_joint_states_.positions[0] = 1.0;
    desired_joint_states_.velocities[0] = 1.5;     // valid pos derivative as well
-    desired_joint_states_.accelerations[0] = 2.9;  // valid pos derivative as well
+    desired_joint_states_.accelerations[0] = 2.0;  // valid pos derivative as well


Pleaes create a new test for this and call it "when_derivative_too_high_expect_lower_effective_limits".

Also make the exact values checks.

saikishor · 2026-07-13T21:15:56Z

-  // velocity max is implicitly already violated due to max_acc * dt > 2.0
+  // if the dt is changed then recalculate the max_acceleration/max_deceleration if implicit vel
+  // exceeds max vel
+  if (std::fabs(dt_seconds - prev_dt_seconds_) > 1e-9)


Is there a chance that the dt won't be changed?

Thanks. Earlier, I was thinking as the enforce fn has dt argument, then the user can change it. But now I think for the real-time control system, dt should be constant. I will remove this comparison of dt and only update the params max_acceleration and max_deceleration to effective acc/dec when impl vel exceeds the max acc/dec.
Pls LMK if this approach is good?

saikishor · 2026-07-13T21:17:10Z

+          RCLCPP_WARN_STREAM_THROTTLE(
+            node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+            "Joint '" << joint_names_[i] << "': dt (" << dt_seconds
+                      << ") is too large for max_acceleration ("
+                      << joint_limits_[i].max_acceleration << ") and max_velocity ("
+                      << joint_limits_[i].max_velocity << "); max_acc * dt = " << implicit_vel
+                      << " exceeds max_velocity");


Please change this to non STREAM version please. STREAM versions are not realtime safe

saikishor · 2026-07-13T21:18:43Z

+          RCLCPP_WARN_STREAM_THROTTLE(
+            node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+            "Joint '" << joint_names_[i] << "': dt (" << dt_seconds
+                      << ") is too large for max_deceleration ("
+                      << joint_limits_[i].max_deceleration << ") and max_velocity ("
+                      << joint_limits_[i].max_velocity << "); max_dec * dt = " << implicit_vel
+                      << " exceeds max_velocity");


Same here non STREAM version

Updated the code for non stream version

Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Replace the time-based delta check with a boolean flag to compute effective acceleration and deceleration limits only once, and update the limits directly within the configuration rather than maintaining separate vectors. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Apply the minimum reduction factor across all joints when implicit velocity exceeds velocity limits to maintain synchronized trajectory execution. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Updates an existing test value and adds a new test case to verify that the joint limiter correctly enforces limits when derivatives exceed configured thresholds. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

sachinkum0009 added 3 commits July 10, 2026 14:04

Add velocity saturation checks and clamping

2c17f0b

Warn when dt is too large for acceleration limits and clamp desired velocity to satisfy max velocity constraints. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Merge branch 'master' into feat/jerk-limit-implicit-vel

8fe1ab0

Remove redundant velocity saturation logic

a87a707

The velocity clipping is handled later in the limiter loop, making this redundant calculation unnecessary. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

sachinkum0009 changed the title ~~Warn user when implicit vel exceeds max vel and add velocity saturation checks and clamping~~ Warn user when implicit vel exceeds max vel Jul 10, 2026

sachinkum0009 marked this pull request as ready for review July 10, 2026 13:32

github-actions Bot requested review from Maverobot, bmagyar, destogl, erickisos and mamueluth July 10, 2026 13:32

sachinkum0009 mentioned this pull request Jul 10, 2026

Enhance JointSaturationLimiter with Jerk limits #3417

Open

6 tasks

sachinkum0009 marked this pull request as draft July 10, 2026 14:01

Recalculate effective acceleration limits based on dt

95f64db

Caches effective acceleration and deceleration limits to ensure they do not exceed the velocity limit when the control cycle duration (dt) changes. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

sachinkum0009 changed the title ~~Warn user when implicit vel exceeds max vel~~ Warn user when implicit vel exceeds max vel and recal effective acc limits Jul 10, 2026

sachinkum0009 commented Jul 10, 2026

View reviewed changes

test added when impl vel exceeds max vel, limits are enforced with

34ef75e

effective acc. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

sachinkum0009 marked this pull request as ready for review July 13, 2026 10:39

github-actions Bot requested review from MarqRazz, bijoua29, mhubii and saikishor July 13, 2026 10:39

destogl previously approved these changes Jul 13, 2026

View reviewed changes

Merge branch 'master' into feat/jerk-limit-implicit-vel

635117e

saikishor requested changes Jul 13, 2026

View reviewed changes

sachinkum0009 added 5 commits July 14, 2026 14:46

Merge branch 'master' into feat/jerk-limit-implicit-vel

4c01a70

Use RCLCPP_WARN_THROTTLE instead of stream variant

4db6261

Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Scale acceleration and deceleration limits uniformly

1e007cc

Apply the minimum reduction factor across all joints when implicit velocity exceeds velocity limits to maintain synchronized trajectory execution. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Add test case for derivative saturation in joint limiter

6925a6e

Updates an existing test value and adds a new test case to verify that the joint limiter correctly enforces limits when derivatives exceed configured thresholds. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

sachinkum0009 dismissed destogl’s stale review via 6925a6e July 14, 2026 14:43

Uh oh!

Conversation

sachinkum0009 commented Jul 10, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Is this user-facing behavior change?

Did you use Generative AI?

Additional Information

TODOs

Uh oh!

codecov Bot commented Jul 10, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

destogl left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

sachinkum0009 commented Jul 10, 2026 •

edited

Loading

codecov Bot commented Jul 10, 2026 •

edited

Loading