From aa7bd28b10532192be43345501bcd9ea6dc20048 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 16 Jun 2025 14:15:57 +0000 Subject: [PATCH 1/3] Use new API of PID class --- .../src/scaled_joint_trajectory_controller.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 94f3cae5d..566cd9a09 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -356,15 +356,17 @@ void ScaledJointTrajectoryController::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.tracking_time_constant = gains.tracking_time_constant; if (pids_[i]) { // update PIDs with gains from ROS parameters -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); -#pragma GCC diagnostic pop + 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); + pids_[i] = std::make_shared(gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat); } ff_velocity_scale_[i] = gains.ff_velocity_scale; } From 69547d04e5d35b44ecb12da2d2fb21eee8368cf0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 16 Jun 2025 17:40:51 +0000 Subject: [PATCH 2/3] Fix pre-commit --- ur_controllers/src/scaled_joint_trajectory_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 566cd9a09..690521cee 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -366,7 +366,8 @@ void ScaledJointTrajectoryController::update_pids() 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.u_clamp_max, gains.u_clamp_min, antiwindup_strat); + pids_[i] = std::make_shared(gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, + antiwindup_strat); } ff_velocity_scale_[i] = gains.ff_velocity_scale; } From efa953450857ef68ed8eb33582723881419e6054 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 17 Jun 2025 11:01:37 +0000 Subject: [PATCH 3/3] Add error_deadband --- ur_controllers/src/scaled_joint_trajectory_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 690521cee..bdc935b0d 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -360,6 +360,7 @@ void ScaledJointTrajectoryController::update_pids() 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