Add derivative filter and discretization methods

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,

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

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_);

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);