diff --git a/doc/release_notes.rst b/doc/release_notes.rst index fe2f7e5d5b..a199c8b8af 100644 --- a/doc/release_notes.rst +++ b/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 `_). * Feed-forward effort trajectories are supported now (`#1200 `_). * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#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 `__). + 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 `__. +* 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 `__). diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 403df0563c..dfcbc02d76 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/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( - 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; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 926366cde6..44748368ef 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/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,