Add new AntiWindup parameters to JTC (#1759)

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>

Author: christophfroehlich

doc/release_notes.rst

@@ -6,4 +6,8 @@ This list summarizes the changes between Jazzy (previous) and Kilted (current) r
 
 force_torque_sensor_broadcaster
 *******************************
-* Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 <https://github.com/ros-controls/ros2_controllers/pull/1647/files>`__.
+* Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 <https://github.com/ros-controls/ros2_controllers/pull/1647/files>`__).
+
+joint_trajectory_controller
+*******************************
+* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 <https://github.com/ros-controls/ros2_controllers/pull/1759>`__).

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -1663,16 +1663,23 @@ void JointTrajectoryController::update_pids()
   for (size_t i = 0; i < num_cmd_joints_; ++i)
   {
     const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i]));
+    control_toolbox::AntiWindupStrategy antiwindup_strat;
+    antiwindup_strat.set_type(gains.antiwindup_strategy);
+    antiwindup_strat.i_max = gains.i_clamp;
+    antiwindup_strat.i_min = -gains.i_clamp;
+    antiwindup_strat.error_deadband = gains.error_deadband;
+    antiwindup_strat.tracking_time_constant = gains.tracking_time_constant;
     if (pids_[i])
     {
       // update PIDs with gains from ROS parameters
-      pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+      pids_[i]->set_gains(
+        gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat);
     }
     else
     {
       // Init PIDs with gains from ROS parameters
       pids_[i] = std::make_shared<control_toolbox::Pid>(
-        gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+        gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat);
     }
     ff_velocity_scale_[i] = gains.ff_velocity_scale;
   }

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -134,6 +134,53 @@ joint_trajectory_controller:
         default_value: 0.0,
         description: "Feed-forward scaling :math:`k_{ff}` of velocity"
       }
+      u_clamp_max: {
+        type: double,
+        default_value: .inf,
+        description: "Upper output clamp."
+      }
+      u_clamp_min: {
+        type: double,
+        default_value: -.inf,
+        description: "Lower output clamp."
+      }
+      i_clamp_max: {
+        type: double,
+        default_value: 0.0,
+        description: "[Deprecated, see antiwindup_strategy] Upper integral clamp."
+      }
+      i_clamp_min: {
+        type: double,
+        default_value: 0.0,
+        description: "[Deprecated, see antiwindup_strategy] Lower integral clamp."
+      }
+      antiwindup_strategy: {
+        type: string,
+        default_value: "legacy",
+        read_only: true,
+        description: "Specifies the anti-windup strategy. Options: 'back_calculation',
+        'conditional_integration', 'legacy' or 'none'. Note that the 'back_calculation' strategy use the
+        tracking_time_constant parameter to tune the anti-windup behavior.",
+        validation: {
+          one_of<>: [[
+            "back_calculation",
+            "conditional_integration",
+            "legacy",
+            "none"
+          ]]
+        }
+      }
+      tracking_time_constant: {
+        type: double,
+        default_value: 0.0,
+        description: "Specifies the tracking time constant for the 'back_calculation' strategy. If
+        set to 0.0 when this strategy is selected, a recommended default value will be applied."
+      }
+      error_deadband: {
+        type: double,
+        default_value: 0.0,
+        description: "Is used to stop integration when the error is within the given range."
+      }
   constraints:
     stopped_velocity_tolerance: {
       type: double,