diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f5630c302e..e9d498edcc 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1764,8 +1764,9 @@ void JointTrajectoryController::update_pids() 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; + // deprecated parameter i_clamp: i_clamp_max and i_clamp_min have precedence over i_clamp + antiwindup_strat.i_max = std::isfinite(gains.i_clamp_max) ? gains.i_clamp_max : gains.i_clamp; + antiwindup_strat.i_min = std::isfinite(gains.i_clamp_min) ? gains.i_clamp_min : -gains.i_clamp; antiwindup_strat.error_deadband = gains.error_deadband; antiwindup_strat.tracking_time_constant = gains.tracking_time_constant; if (pids_[i]) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 44748368ef..b06c8c2e26 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -132,8 +132,18 @@ joint_trajectory_controller: } i_clamp: { type: double, - default_value: 0.0, - description: "Integral clamp. Symmetrical in both positive and negative direction." + default_value: .inf, + description: "[Deprecated, use i_clamp_max/i_clamp_min] Integral clamp. Symmetrical in both positive and negative direction." + } + i_clamp_max: { + type: double, + default_value: .inf, + description: "Upper integral clamp." + } + i_clamp_min: { + type: double, + default_value: -.inf, + description: "Lower integral clamp." } ff_velocity_scale: { type: double, @@ -150,16 +160,6 @@ joint_trajectory_controller: 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", diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 4ba095feee..eecceeee70 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -191,6 +191,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param { rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); + SetPidParameters(0.0, 1.0); traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); traj_controller_->get_node()->set_parameter(rclcpp::Parameter("update_rate", 10)); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b4c7cdf5f6..b05c7a8116 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -279,11 +279,18 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_ = std::make_shared(); auto node_options = rclcpp::NodeOptions(); + // read-only parameters have to be set before init std::vector parameter_overrides; parameter_overrides.push_back(rclcpp::Parameter("joints", joint_names_)); parameter_overrides.push_back( rclcpp::Parameter("command_interfaces", command_interface_types_)); parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); + // avoid deprecation warning for legacy (default) antiwindup strategy + for (const auto & joint : joint_names_) + { + parameter_overrides.push_back( + rclcpp::Parameter("gains." + joint + ".antiwindup_strategy", "none")); + } parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); traj_controller_->set_node_options(node_options); @@ -303,7 +310,7 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_p(prefix + ".p", p_value); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); - const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); + const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 1000.0); const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); } diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 0ec150c5ab..46d7d3748a 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -85,12 +85,12 @@ pid_controller: } i_clamp_max: { type: double, - default_value: 0.0, + default_value: .inf, description: "[Deprecated, see antiwindup_strategy] Upper integral clamp." } i_clamp_min: { type: double, - default_value: 0.0, + default_value: -.inf, description: "[Deprecated, see antiwindup_strategy] Lower integral clamp." } antiwindup_strategy: { diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index f8b27f1d49..87b0cb3176 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -2,8 +2,8 @@ ros__parameters: # TODO(christohfroehlich): Remove this global parameters once the deprecated antiwindup parameters are removed. gains: - joint1: {antiwindup_strategy: "none", i_clamp_max: .inf, i_clamp_min: -.inf} - joint2: {antiwindup_strategy: "none", i_clamp_max: .inf, i_clamp_min: -.inf} + joint1: {antiwindup_strategy: "none",} + joint2: {antiwindup_strategy: "none",} test_pid_controller: ros__parameters: diff --git a/pid_controller/test/pid_controller_preceding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml index 673abfe08e..b5ec89c2f5 100644 --- a/pid_controller/test/pid_controller_preceding_params.yaml +++ b/pid_controller/test/pid_controller_preceding_params.yaml @@ -9,3 +9,6 @@ test_pid_controller: reference_and_state_dof_names: - joint1state + + gains: + joint1: {antiwindup_strategy: "none"}