Add decelerate to stop functionality when trajectory is canceled or preempted (#2163)

Author: MarqRazz

doc/release_notes.rst

@@ -44,6 +44,7 @@ joint_trajectory_controller
   don't contain velocity / acceleration information, but the trajectory does. This way, the segment
   up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043
   <https://github.com/ros-controls/ros2_controllers/pull/2043>`_)
+* Added decelerate-to-stop functionality when a trajectory is canceled or preempted. Instead of immediately holding position, the controller can now smoothly decelerate each joint to a stop using the per-joint ``max_deceleration_on_cancel`` parameter. (`#2163 <https://github.com/ros-controls/ros2_controllers/pull/2163>`_)
 
 pid_controller
 **************

joint_trajectory_controller/CMakeLists.txt

@@ -106,7 +106,7 @@ if(BUILD_TESTING)
   ament_add_gmock(test_trajectory_actions
     test/test_trajectory_actions.cpp
   )
-  set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300)
+  set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 330)
   target_link_libraries(test_trajectory_actions
     joint_trajectory_controller
     ros2_control_test_assets::ros2_control_test_assets)

joint_trajectory_controller/doc/decelerate_hold_position.png

@@ -0,0 +1,83 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/decelerate_on_cancel.rst
+
+.. _joint_trajectory_controller_decelerate_on_cancel:
+
+Decelerate on cancel
+====================
+
+By default, when a trajectory is canceled or preempted, the controller immediately holds the current position.
+This can cause problems for hardware that is moving at high velocity, as the abrupt stop may trigger faults or cause excessive wear.
+
+When the ``decelerate_on_cancel`` feature is enabled, the controller instead generates a smooth stop trajectory that decelerates each joint to zero velocity using a constant deceleration profile before holding position.
+
+.. image:: decelerate_hold_position.png
+   :alt: Default behavior: immediate hold position on cancel
+
+*Default behavior: the controller instantly sets the command position to the current position when a trajectory is canceled, causing an abrupt stop.*
+
+.. image:: decelerate_to_stop.png
+   :alt: Decelerate to stop behavior on cancel
+
+*With decelerate on cancel enabled: the controller generates a ramped stop trajectory that smoothly decelerates each joint before holding position.*
+
+How it works
+------------
+
+When a trajectory is canceled or preempted and ``decelerate_on_cancel`` is enabled, the controller:
+
+1. Reads the current velocity of each joint from the velocity state interface.
+2. Computes the stopping distance and time for each joint using its configured ``max_deceleration_on_cancel`` value:
+
+   .. math::
+
+      t_{stop} = \frac{|v|}{a_{max}}
+
+   .. math::
+
+      d_{stop} = \frac{v \cdot t_{stop}}{2}
+
+   where :math:`v` is the current velocity and :math:`a_{max}` is the configured maximum deceleration.
+
+3. Generates a trajectory with intermediate waypoints that linearly ramp velocity to zero over the computed stopping time.
+4. Appends a final hold-position point at the computed stop position.
+
+Each joint decelerates independently based on its own ``max_deceleration_on_cancel`` value, but the trajectory is synchronized so all joints finish at the same time (the slowest joint's stop time).
+
+Requirements
+------------
+
+* The hardware must provide a ``velocity`` state interface for all joints in the controller. If velocity state feedback is not available, the controller falls back to the default hold-position behavior.
+* Each joint must have a valid (greater than zero) ``max_deceleration_on_cancel`` value. Joints with a value of ``0.0`` cause the controller to fall back to hold-position behavior.
+
+Configuration
+-------------
+
+Enable the feature by setting ``constraints.decelerate_on_cancel`` to ``true`` and providing a ``max_deceleration_on_cancel`` value (in rad/s^2 or m/s^2) for each joint under ``constraints.<joint_name>``:
+
+.. code-block:: yaml
+
+   controller_name:
+     ros__parameters:
+       joints:
+         - joint_1
+         - joint_2
+         - joint_3
+
+       constraints:
+         decelerate_on_cancel: true
+         stopped_velocity_tolerance: 0.01
+         goal_time: 0.0
+         joint_1:
+           max_deceleration_on_cancel: 10.0
+         joint_2:
+           max_deceleration_on_cancel: 3.0
+         joint_3:
+           max_deceleration_on_cancel: 6.0
+
+.. note::
+
+   Both ``decelerate_on_cancel`` and ``max_deceleration_on_cancel`` are read-only parameters. They can only be set at controller configuration time and cannot be changed at runtime.
+
+.. note::
+
+   Choose ``max_deceleration_on_cancel`` values that are within the physical limits of your hardware. Values that are too high may still cause faults, while values that are too low will result in longer stopping distances.

joint_trajectory_controller/doc/decelerate_on_cancel.rst

@@ -1,5 +1,6 @@
 constraints:
     Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message.
+    Also contains the ``decelerate_on_cancel`` option and per-joint ``max_deceleration_on_cancel`` values for smooth stopping behavior (see :ref:`joint_trajectory_controller_decelerate_on_cancel`).
 
 gains: |
   The parameters are used to configure PID loops for the ``velocity`` or ``effort``-only command interfaces. This structure contains the controller gains for every joint with following the control laws

joint_trajectory_controller/doc/decelerate_to_stop.png

@@ -61,6 +61,8 @@ Other features
 
 * Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments.
 
+* Optional smooth deceleration on cancel: Instead of abruptly holding position, the controller can decelerate joints to a stop using configurable per-joint deceleration limits. See :ref:`joint_trajectory_controller_decelerate_on_cancel`.
+
 
 Using Joint Trajectory Controller(s)
 ------------------------------------
@@ -209,6 +211,7 @@ Further information
    :titlesonly:
 
    Trajectory Representation <trajectory.rst>
+   Decelerate on cancel <decelerate_on_cancel.rst>
    Speed scaling <speed_scaling.rst>
    joint_trajectory_controller Parameters <parameters.rst>
    rqt_joint_trajectory_controller <../../rqt_joint_trajectory_controller/doc/userdoc.rst>

joint_trajectory_controller/doc/parameters_context.yaml

@@ -150,6 +150,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // reserved storage for result of the command when closed loop pid adapter is used
   std::vector<double> tmp_command_;
 
+  // If true, enable calculations to stop all joints using constant deceleration
+  bool should_decelerate_on_cancel_ = false;
+  // reserved storage for the max deceleration values
+  std::vector<double> max_decel_;
+  // reserved storage for each joints max stopping time
+  std::vector<double> stop_time_;
+  // reserved storage for each joints hold position at stop
+  std::vector<double> hold_position_;
+  // reserved storage for each joints stop direction
+  std::vector<double> stop_direction_;
+  // reserved storage for the stop trajectory
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> stop_trajectory_;
+
   // Things around speed scaling
   std::atomic<double> scaling_factor_{1.0};
   std::atomic<double> scaling_factor_cmd_{1.0};
@@ -238,6 +251,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
    */
   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_hold_position();
 
+  /** @brief decelerate at constant rate to a holding position with
+   * zero velocity and acceleration as new command
+   */
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> decelerate_to_hold_position();
+
   /** @brief set last trajectory point to be repeated at success
    *
    * no matter if it has nonzero velocity or acceleration