Add decelerate to stop functionality when trajectory is canceled or preempted

[MarqRazz]

Title says it all. The old behavior of the JTC is to set the robots command position to its current pose at the time the trajectory is canceled.

For my hardware this frequently causes it to fault because it instantly sets the joints velocity to zero no matter how fast it is moving. Below is a plot of the new behavior...

I added a few parameters to the constraints struct to opt into the new functionality but once the URDF supports specifying joint acceleration we could use those values if these are not specified.

constraints:
  decelerate_on_cancel: true
  stopped_velocity_tolerance: 0.008
  goal_time: 0.0
  joint_1:
    goal: 0.005
    max_deceleration_on_cancel: 10.0
  joint_2:
    goal: 0.005
    max_deceleration_on_cancel: 3.0
  joint_3:
    goal: 0.005
    max_deceleration_on_cancel: 6.0
  joint_4:
    goal: 0.005
    max_deceleration_on_cancel: 10.0
  joint_5:
    goal: 0.005
    max_deceleration_on_cancel: 10.0
  joint_6:
    goal: 0.005
    max_deceleration_on_cancel: 17.0

The new tests check for ...

• Invalid max_deceleration:

5: [test_joint_trajectory_controller 1770759765.603637493]: Canceling active action goal because cancel callback received.
5: [test_joint_trajectory_controller 1770759765.603646091]: Joint [joint1] invalid max_deceleration_on_cancel [0.0]. Falling back to hold position.

• Hardware does not have a velocity state interface:

5: [test_joint_trajectory_controller 1770761095.581896746]: Canceling active action goal because cancel callback received.
5: [test_joint_trajectory_controller 1770761095.581907052]: Hardware does not support velocity state interface, falling back to hold position.

• Hardware is already stopped (or is reporting zeros like several of the tests do)

5: [test_joint_trajectory_controller 1770761097.789958878]: Canceling active action goal because cancel callback received.
5: [test_joint_trajectory_controller 1770761097.789967357]: Joint [joint1] decel [10.000], stop dist [0.0000], initial vel [0.0000], initial pos [2.0826], hold pos [2.0826], time to stop [0.0000]
5: [test_joint_trajectory_controller 1770761097.789995287]: Joint [joint2] decel [10.000], stop dist [0.0000], initial vel [0.0000], initial pos [2.9553], hold pos [2.9553], time to stop [0.0000]
5: [test_joint_trajectory_controller 1770761097.790008351]: Joint [joint3] decel [10.000], stop dist [0.0000], initial vel [0.0000], initial pos [3.8280], hold pos [3.8280], time to stop [0.0000]
5: [test_joint_trajectory_controller 1770761097.790021810]: Created ramped stop trajectory with [1] points and max time to stop [0.000] sec

• Hardware has non-zero velocity and needs a ramp generated to stop gracefully

5: [test_joint_trajectory_controller 1770761179.755913527]: Canceling active action goal because cancel callback received.
5: [test_joint_trajectory_controller 1770761179.755922035]: Joint [joint1] decel [10.000], stop dist [0.5749], initial vel [3.3908], initial pos [2.0849], hold pos [2.6598], time to stop [0.3391]
5: [test_joint_trajectory_controller 1770761179.755956492]: Joint [joint2] decel [10.000], stop dist [0.4967], initial vel [3.1517], initial pos [2.9575], hold pos [3.4541], time to stop [0.3152]
5: [test_joint_trajectory_controller 1770761179.755969589]: Joint [joint3] decel [10.000], stop dist [0.4241], initial vel [2.9125], initial pos [3.8300], hold pos [4.2541], time to stop [0.2912]
5: [test_joint_trajectory_controller 1770761179.755984729]: Created ramped stop trajectory with [35] points and max time to stop [0.339] sec

[codecov]

Codecov Report

❌ Patch coverage is 94.27481% with 15 lines in your changes missing coverage. Please review.
✅ Project coverage is 85.08%. Comparing base (665c60c) to head (42980df).

Files with missing lines	Patch %	Lines
...ory_controller/src/joint_trajectory_controller.cpp	83.72%	7 Missing and 7 partials ⚠️
...ory_controller/test/test_trajectory_controller.cpp	99.13%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2163      +/-   ##
==========================================
+ Coverage   84.92%   85.08%   +0.15%     
==========================================
  Files         153      153              
  Lines       14990    15247     +257     
  Branches     1291     1313      +22     
==========================================
+ Hits        12731    12973     +242     
- Misses       1785     1792       +7     
- Partials      474      482       +8     

Flag	Coverage Δ
unittests	85.08% <94.27%> (+0.15%)	⬆️

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

Files with missing lines	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	50.00% <ø> (ø)
...ectory_controller/test/test_trajectory_actions.cpp	97.64% <100.00%> (+0.24%)	⬆️
...ntroller/test/test_trajectory_controller_utils.hpp	84.83% <ø> (+0.04%)	⬆️
...ory_controller/test/test_trajectory_controller.cpp	99.73% <99.13%> (-0.07%)	⬇️
...ory_controller/src/joint_trajectory_controller.cpp	83.76% <83.72%> (-0.13%)	⬇️

🚀 New features to boost your workflow:

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

[saikishor]

Very nice approach.
Thanks for such a nice implementation.

I've some nitpicks and comments here and there.

I'm curious to know what happens, if the declaration_on_cancel is only set for some joints of the JTC, but not all.

Thank you

[saikishor]

Maybe it is better to pass an object by reference where the method can fill in information, this way we can avoid memory allocation in RT loop

[MarqRazz]

Something is going to need to preallocate the max size for a worst case stop trajectory which could be quite large depending on the users settings. For example if they are running at 1kHz vs 100 Hz they will need a lot more space for it. Also depends on how fast each joint can go so we will need to plan for the fastest joint and user defined max deceleration.

const size_t num_points = static_cast<size_t>(std::ceil(max_time_to_stop / sample_period)) + 1;
traj->points.reserve(num_points);

Right now decelerate_to_stop is a free function in the trajectory.cpp file. Who should be the owner of this memory? The JTC or the Trajectory class (assuming we move it into the class).

[MarqRazz]

I pre-allocated the variables based on the URDF defined joint limits and added the ability to dynamically grow if users request trajectories that are faster than defined there (most of the tests do this).

[pastoriomarco]

Hi!
I tested this feature with moveit (quite painful to set it up with ros2_controllers rolling... if you have some instructions to load it all more simply it'd be great).

For what I see the decelerated stop works well, and a decelerated stop surely is a way better approach than a no-ramp hard stop.

I think this feature is a must-have. It's very common in any industrial robot to have a stop functionality that ramps down with a given max acceleration for each axis. I don't know if I can do anything more than give my feedback here to help approve this PR, the code and comments look good to me.

This said, if I may, the biggest problem I see here is that it's not on-path. After testing with different accelerations (even very low ones to amplify the edge cases), it's clear that this could lead to undesired effects, such as path overshoots and off-path positions.
On modern robots the standard for a normal stop function (and even a safety-stop function in some brands) is an on-path stop.

I tried to implement an on-path soft-stop function in my branch, see #2111 for details and link to code. It works on humble, jazzy, kilted and rolling, but to be honest it's not PR-ready and would need a good polish (lots of copy paste of same code structure in various parts that I'd like to refine). It would be very useful to at least understand if there's interest in merging this kind of functionality and I'll work on it (after discussing the implementation details required, obviously)

[MarqRazz]

Hi! I tested this feature with moveit (quite painful to set it up with ros2_controllers rolling... if you have some instructions to load it all more simply it'd be great).

Sorry that is not something we really have for ros2_control. We don't maintain configurations between ros2_control and other packages that use it and my companies robot configuration is private. In my first post to the PR I shared the configuration values you need to set in your control.yaml for the JTC so hopefully that made it a little easier.

This said, if I may, the biggest problem I see here is that it's not on-path. After testing with different accelerations (even very low ones to amplify the edge cases), it's clear that this could lead to undesired effects, such as path overshoots and off-path positions. On modern robots the standard for a normal stop function (and even a safety-stop function in some brands) is an on-path stop.

This functionality is similar to a Protective or Emergency stop event except the brakes don't engage or power off the servos. This is the safest thing to do, you said stop!

I can't protect users from specifying poor values for deceleration. If the trajectory requested to execute has larger accelerations than they set in the JTC which one should it choose? The user set specific settings for how to handle a cancel event shouldn't they always be applied? Technically the JTC does not enforce or validate a given trajectory against the URDF (which doesn't contain all of the joints dynamic limits) it is running on before it executes it so users are allowed to ask for things that the hardware can struggle or fail to execute.

[MarqRazz]

I'm curious to know what happens, if the declaration_on_cancel is only set for some joints of the JTC, but not all.

If any of the joints have zero values for max_deceleration_on_cancel it falls back to set_hold_position.

[pastoriomarco]

Sorry that is not something we really have for ros2_control. We don't maintain configurations between ros2_control and other packages that use it and my companies robot configuration is private. In my first post to the PR I shared the configuration values you need to set in your control.yaml for the JTC so hopefully that made it a little easier.

Yep, it made it a lot easier indeed! The hard part was to have all compile and work together with latest(ish) rolling ros2_controller, especially MoveIt and related packages.

This functionality is similar to a Protective or Emergency stop event except the brakes don't engage or power off the servos. This is the safest thing to do, you said stop!

I can't protect users from specifying poor values for deceleration. If the trajectory requested to execute has larger accelerations than they set in the JTC which one should it choose? The user set specific settings for how to handle a cancel event shouldn't they always be applied? Technically the JTC does not enforce or validate a given trajectory against the URDF (which doesn't contain all of the joints dynamic limits) it is running on before it executes it so users are allowed to ask for things that the hardware can struggle or fail to execute.

As I said, this functionality is simply a must-have, no doubt about it! It's one of the available modes of the protective/emergency stop in modern robots indeed. It may be ok for an emergency stop, but surely it's not something I want on a standard stop: it's not on-path, potentially risky for attached workpieces and arguably too hard on the hardware for a standard stop (depending on payload and on the tradeoff between as close to on-path as possible and softer deceleration). I tried too soft accelerations just to confirm the behavior, but even setting everything perfectly it just has several downsides for non-emergency stops.

This kind of stop is actually something that I'm not using in any of my applications, not even for emergency stops, simply because I design the layout so it's never necessary to decelerate with max acceleration: simplifying, what's needed for certification is the robot to be stopped within a certain time and for it to never exit a determined area during operation.

That's why I feel the need for a soft-stop function too (not instead), it's really a primary functionality of any robotic application.