Add new AntiWindup parameters to JTC (backport #1759) (#1762)

Co-authored-by: Christoph Fröhlich <[email protected]>
Co-authored-by: Sai Kishor Kothakota <[email protected]>

Author: 3 people

doc/release_notes.rst

@@ -54,6 +54,8 @@ joint_trajectory_controller
 * Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 <https://github.com/ros-controls/ros2_controllers/pull/1231>`_).
 * Feed-forward effort trajectories are supported now (`#1200 <https://github.com/ros-controls/ros2_controllers/pull/1200>`_).
 * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 <https://github.com/ros-controls/ros2_controllers/pull/1525>`_).
+* 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>`__).
+
 
 mecanum_drive_controller
 ************************
@@ -83,4 +85,4 @@ gpio_controllers
 
 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/src/joint_trajectory_controller.cpp

@@ -1673,16 +1673,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

@@ -140,6 +140,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,