Enhance JointSaturationLimiter with Jerk limits by sachinkum0009 · Pull Request #3417 · ros-controls/ros2_control

sachinkum0009 · 2026-06-26T09:02:59Z

Description

This PR added the Jerk Limits to JointSaturationLimiter.

Summary of changes

Refactor code to functions for readability
Pre-allocate vector to avoid malloc when calling enforce fn
Implemented Jerk Limiter to check limitation and backward recalculate safe acc, vel and pos
Tests added for pos only, vel only, acc only, pos + vel, vel + acc, pos + vel + acc.
Warn users when implicit vel exceed max velocity and then reclamp velocity to max velocity
Post Jerk limit recalculate acc, vel, pos

Fixes # (ros-controls/ros2_controllers#2434)

Is this user-facing behavior change?

Did you use Generative AI?

Used Opencode to understand the code and implement tests.

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.

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

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

…re by reorganizing conditional checks for desired position, velocity, and acceleration Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

… by using boolean flags for position, velocity, acceleration, and deceleration limits, and streamline the clamp_joint_limits method. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

…position_limit methods with detailed step-by-step explanations Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

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

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

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

codecov · 2026-06-26T09:25:36Z

Codecov Report

❌ Patch coverage is 83.23171% with 55 lines in your changes missing coverage. Please review.
✅ Project coverage is 89.47%. Comparing base (4bcce09) to head (b2c4751).

Files with missing lines	Patch %	Lines
joint_limits/src/joint_saturation_limiter.cpp	78.03%	35 Missing and 12 partials ⚠️
...oint_limits/test/test_joint_saturation_limiter.cpp	92.23%	0 Missing and 8 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #3417      +/-   ##
==========================================
- Coverage   89.51%   89.47%   -0.05%     
==========================================
  Files         164      164              
  Lines       21265    21479     +214     
  Branches     1658     1687      +29     
==========================================
+ Hits        19036    19218     +182     
- Misses       1530     1555      +25     
- Partials      699      706       +7

Flag	Coverage Δ
unittests	`89.47% <83.23%> (-0.05%)`	⬇️

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

Files with missing lines	Coverage Δ
...nt_limits/include/joint_limits/data_structures.hpp	`80.55% <ø> (ø)`
.../include/joint_limits/joint_saturation_limiter.hpp	`100.00% <100.00%> (ø)`
...oint_limits/test/test_joint_saturation_limiter.cpp	`91.49% <92.23%> (+0.25%)`	⬆️
joint_limits/src/joint_saturation_limiter.cpp	`81.09% <78.03%> (-4.29%)`	⬇️

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

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

…reference in JointSaturationLimiter methods Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

…ter acceleration limiting in JointSaturationLimiter Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

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

mergify · 2026-06-29T09:38:45Z

This pull request is in conflict. Could you fix it @sachinkum0009?

Added a re-check mechanism after calculating desired acceleration based on jerk-limited values. This ensures that velocity and acceleration do not violate joint limits due to the influence of jerk-limited acceleration or deceleration calculations. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Adjusts the logic for calculating safe target velocities when a joint limit is hit to ensure smoother deceleration towards the limit. This replaces the placeholder calculation with physics-based limits considering deceleration and time constraints. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Removes detailed, multi-step comments from the documentation of the `clamp_joint_limits` and `handle_braking_near_position_limit` methods. These functions' internal workings are complex enough that the previous verbose description became overly long and hard to parse. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Updates the joint saturation limiter to refactor redundant and complex code blocks related to re-clamping velocity after acceleration limiting. Introduces a local lambda, `post_velocity_check_and_clamp`, to centralize the logic for clamping velocity, recalculating derived values (acceleration and position), and ensuring consistency when multiple limits are hit sequentially. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Updates `clamp_joint_limits` in both header and source files to accept and use the availability of current joint velocities for more accurate trajectory limiting. The warning message when acceleration limits are configured but current velocity is absent is also updated accordingly. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

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

sachinkum0009 · 2026-07-04T14:33:41Z

Hi @saikishor @christophfroehlich

Sorry to ping you.
I wanted to request if you can review this PR and give your suggestions. This PR integrates the jerk limit checks to JointSaturationLimiter and backward calculate the acceleration, velocity and position.
I have refactored the code a little bit and updated the test for pos, vel, acc, pos+vel, vel+acc, pos+vel+acc limits crossed.

I have self reviewed it couple of times. I know that you are busy, it would really help me learn and contribute to ros2 control and become a maintainer in future.
Thanks

Ensure position limits are only applied when configured and re-clamp velocity limits after acceleration and jerk computations to maintain validity. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

destogl

As general comments.

Let's move the following in a separate PR:

Refactor code to functions for readability
Pre-allocate vector to avoid malloc when calling enforce fn

Also separate:

Warn users when implicit vel exceed max velocity and then reclamp velocity to max velocity

And then rebase this PR on those changes it will make them more readable and simpler to merge.

I find this maybe too simple implementation. Are you sure there are no more cases when we have jerk limits. Can you make the tests of all possible cases in robot trajectory where jerk limits will be important?

destogl · 2026-07-08T18:43:27Z

    prev_command_ = current_joint_states;
+    const size_t num_joints = this->number_of_joints_;
+
+    desired_pos_.assign(num_joints, 0.0);


This is a very good catch! I would like to see this in a separate PR, as this seems to be merged very fast.

Separate PR created for this.

destogl · 2026-07-08T18:44:10Z

  std::mutex mutex_;
+
+private:
+  // Cached vectors to eliminate dynamic memory allocation (malloc) in the real-time execution loop


@saikishor is this true that we had the memory allocations previously? Can this be?

destogl · 2026-07-08T18:46:15Z

+   */
+  void clamp_joint_limits(
+    const bool has_desired_position, const bool has_desired_velocity,
+    const bool has_desired_acceleration, const bool has_current_velocity,


This seems to be part of the restructuring. I would like to see this in a separate PR. This is hard to figure out what is important and why.

Separate PR created for restructuring.

destogl · 2026-07-08T18:46:52Z

+   * Handles the braking near position limit
+   */
+  void handle_braking_near_position_limit(
+    const std::vector<double> & current_joint_velocities, double dt_seconds,


why only "position" and "velocity" input?

destogl · 2026-07-08T18:47:44Z

+  const std::vector<double> & current_joint_velocities,
+  bool & braking_near_position_limit_triggered, const double dt_seconds);
+
+template <>


this is template for what exactly?

destogl · 2026-07-08T20:03:41Z

+    }
+
+    // Re-clamp desired velocity after acceleration/jerk may have recomputed it
+    if (


this will be done always, why do we then have it above?

destogl · 2026-07-08T20:03:55Z

+    {
+      clamp_vel_limit();
+    }
+    // Re-clamp desired position after acceleration/jerk may have recomputed it


Also this one.

destogl · 2026-07-08T20:05:31Z

      double stopping_distance =
-        std::fabs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel));
+        std::fabs((-desired_vel_[index] * desired_vel_[index]) / (2 * stopping_deccel));
      // compute stopping duration at stopping_deccel


OK, but what happens with jerk in that case?

destogl · 2026-07-08T20:06:04Z

    // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05)
-    std::vector expected_ret = {true, true, true, false};
-    for (auto i = 0u; i < 4; ++i)
+    // cycles 0-3 apply limits (braking + jerk), cycles 4-5 settle at stop


I what to see a new test with jerk. Current test should stay the same, as we should test also when no jerk limits are defined.

destogl · 2026-07-08T20:07:55Z

+
+    rclcpp::Duration period(0, 50000000);
+
+    current_joint_states_.velocities[0] = 1.0;


give here more explanation on the numbers in a comment of the test. For all tests.

sachinkum0009 · 2026-07-10T13:35:33Z

Thank you so much for the review.
Created two separate PR

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

sachinkum0009 added 9 commits June 22, 2026 18:24

Refactor JointSaturationLimiter to use cached vectors for desired and…

8363bef

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

Add clamp_joint_limits and handle_braking_near_position_limit methods…

c0d7a0d

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

Refactor clamp_joint_limits method to improve readability and structu…

167ea8a

…re by reorganizing conditional checks for desired position, velocity, and acceleration Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Refactor JointSaturationLimiter to improve limit tracking and logging…

11756d0

… by using boolean flags for position, velocity, acceleration, and deceleration limits, and streamline the clamp_joint_limits method. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Enhance documentation for clamp_joint_limits and handle_braking_near_…

763310b

…position_limit methods with detailed step-by-step explanations Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Add jerk limits support to JointSaturationLimiter with corresponding …

886bb9c

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

Fix typos in test names and update expected results for JointSaturati…

e75ee32

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

pre commit fix

4a9acd5

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

Add tests for jerk limit enforcement in JointSaturationLimiter

3051238

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

github-actions Bot requested review from Maverobot, bijoua29, destogl, homalozoa and saikishor June 26, 2026 09:03

sachinkum0009 added 4 commits June 26, 2026 11:26

Refactor parameter passing for current_joint_velocities to use const …

7f2ecf9

…reference in JointSaturationLimiter methods Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Add warning for implicit velocity violations and re-clamp velocity af…

54a9c5c

…ter acceleration limiting in JointSaturationLimiter Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Add test for velocity re-clamping when acceleration limits are exceeded

2ac2b0f

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

test added for pos and acc when jerk exceeds

d3fccd6

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

christophfroehlich added this to Contributing Jun 29, 2026

christophfroehlich removed this from Contributing Jun 29, 2026

christophfroehlich added this to Review triage Jun 29, 2026

christophfroehlich moved this to Needs review in Review triage Jun 29, 2026

sachinkum0009 added 6 commits June 29, 2026 13:25

Merge branch 'master' into feat/jtc-jerk-limits

9653d5b

Merge branch 'master' into feat/jtc-jerk-limits

b53bf25

sachinkum0009 added 3 commits July 3, 2026 17:28

Refactor: Use const for stopping duration calculation

b61a15a

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

Refactor: Simplify joint position and velocity clamping logic

cb7e3f0

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

sachinkum0009 added 2 commits July 6, 2026 23:12

Add safety checks for joint position and velocity limits

89d71c5

Ensure position limits are only applied when configured and re-clamp velocity limits after acceleration and jerk computations to maintain validity. Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Merge branch 'master' into feat/jtc-jerk-limits

f96151e

destogl requested changes Jul 8, 2026

View reviewed changes

github-project-automation Bot moved this from Needs review to WIP in Review triage Jul 8, 2026

sachinkum0009 added 2 commits July 13, 2026 20:04

Merge branch 'master' into feat/jtc-jerk-limits

735a1f2

Merge branch 'master' into feat/jtc-jerk-limits

b6442a6

sachinkum0009 force-pushed the feat/jtc-jerk-limits branch from 6a56d11 to b6442a6 Compare July 17, 2026 18:00

sachinkum0009 added 4 commits July 19, 2026 20:56

Clamp for final position added when handle brake

947bac0

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

Merge branch 'master' into feat/jtc-jerk-limits

ee8960b

Merge branch 'master' into feat/jtc-jerk-limits

4334f2d

todo added

b2c4751

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


		rclcpp::Duration period(0, 50000000);

		current_joint_states_.velocities[0] = 1.0;

Uh oh!

Conversation

sachinkum0009 commented Jun 26, 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 Jun 26, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

mergify Bot commented Jun 29, 2026

Uh oh!

sachinkum0009 commented Jul 4, 2026

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!

sachinkum0009 commented Jul 10, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

sachinkum0009 commented Jun 26, 2026 •

edited

Loading

codecov Bot commented Jun 26, 2026 •

edited

Loading