diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 6e77f23867..0bdd000b34 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -60,6 +60,16 @@ pid_controller: default_value: 0.0, description: "Derivative gain for PID" } + derivative_filter_time: { + type: double, + default_value: 0.0, + description: "Time constant (seconds) of the first-order low-pass in the derivative path. + Continuous form: C_D(s) = D * s / (1 + s * derivative_filter_time). Smaller values => less + filtering (higher bandwidth); derivative_filter_time = 0 gives an ideal (unfiltered) + differentiator (not recommended). In discrete time the final difference equation depends on + 'derivative_method' and sample time T.", + validation: { gt_eq<>: [0.0] } + } u_clamp_max: { type: double, default_value: .inf, @@ -94,6 +104,38 @@ pid_controller: ]] } } + integration_method: { + type: string, + default_value: "forward_euler", + description: "Discrete-time scheme for the integral (I) term. Options: 'forward_euler' + (explicit Euler), 'backward_euler' (implicit Euler), or 'trapezoidal' + (bilinear/Tustin). In general, 'trapezoidal' offers better accuracy and + phase characteristics; 'backward_euler' is more numerically stable for larger + time steps (more dissipative).", + validation: { + one_of<>: [[ + "forward_euler", + "backward_euler", + "trapezoidal" + ]] + } + } + derivative_method: { + type: string, + default_value: "forward_euler", + description: "Selects the discrete-time scheme used to discretize the D term of the PID. + Options: 'forward_euler' (explicit Euler), 'backward_euler' (implicit Euler), or + 'trapezoidal' (bilinear/Tustin). In general, 'trapezoidal' offers better accuracy and phase + characteristics; 'backward_euler' is more numerically stable for larger + time steps (more dissipative).", + validation: { + one_of<>: [[ + "forward_euler", + "backward_euler", + "trapezoidal" + ]] + } + } tracking_time_constant: { type: double, default_value: 0.0, diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index f8b27f1d49..6fb896644c 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -90,3 +90,32 @@ test_save_i_term_on: gains: joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: true} + +test_pid_controller_discretization_gains: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: + p: 1.0 + i: 2.0 + d: 3.0 + derivative_filter_time: 1.0 + u_clamp_max: 5.0 + u_clamp_min: -5.0 + i_clamp_max: 4.0 + i_clamp_min: -4.0 + antiwindup_strategy: "back_calculation" + integration_method: "backward_euler" + derivative_method: "backward_euler" + tracking_time_constant: 0.1 + error_deadband: 0.01 + feedforward_gain: 1.5 + angle_wraparound: true + save_i_term: false + activate_state_publisher: true diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index d1e9754830..f3a82a7dc7 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -193,6 +193,51 @@ TEST_F(PidControllerTest, reactivate_success) controller_interface::return_type::OK); } +TEST_F(PidControllerTest, all_parameters_set_configure_success_discretization_gains) +{ + SetUpController("test_pid_controller_discretization_gains"); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_THAT(controller_->reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 3.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].derivative_filter_time, 1.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].u_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].u_clamp_min, -5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 4.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -4.0); + ASSERT_EQ( + controller_->params_.gains.dof_names_map[dof_name].antiwindup_strategy, "back_calculation"); + ASSERT_EQ( + controller_->params_.gains.dof_names_map[dof_name].integration_method, "backward_euler"); + ASSERT_EQ( + controller_->params_.gains.dof_names_map[dof_name].derivative_method, "backward_euler"); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].tracking_time_constant, 0.1); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].error_deadband, 0.01); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].angle_wraparound, true); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].save_i_term, false); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].activate_state_publisher, true); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); +} + /** * @brief Check the update logic in non chained mode with feedforward gain is 0 * @@ -669,13 +714,18 @@ TEST_F(PidControllerTest, test_save_i_term_on) // check the command value // error = ref - state = 100.001, error_dot = error/ds = 10000.1, // p_term = 100.001 * 1, i_term = zero at first update, d_term = error/ds = 10000.1 * 3 - // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.301 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30100.3010 const double expected_command_value = 30100.3010; double actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); + // second update, the i_term should be non-zero now + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // deactivate the controller and set command=state ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); controller_->set_reference(dof_state_values_); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 2a3f60464d..280a3d8be9 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -53,6 +53,7 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, all_parameters_set_configure_success); FRIEND_TEST(PidControllerTest, activate_success); FRIEND_TEST(PidControllerTest, reactivate_success); + FRIEND_TEST(PidControllerTest, all_parameters_set_configure_success_discretization_gains); FRIEND_TEST(PidControllerTest, test_update_logic_zero_feedforward_gain); FRIEND_TEST(PidControllerTest, test_update_chained_non_zero_feedforward_gain); FRIEND_TEST(PidControllerTest, test_update_chained_changing_feedforward_gain);