Reset the PIDs states when controller is at rest

pid_controller/include/pid_controller/pid_controller.hpp

@@ -86,6 +86,8 @@ class PidController : public controller_interface::ChainableControllerInterface
   // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
   std::vector<double> feedforward_gain_;
 
+  double reset_pid_time_;
+
   // Command subscribers and Controller State publisher
   rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
   realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;

pid_controller/src/pid_controller.cpp

@@ -533,6 +533,23 @@ controller_interface::return_type PidController::update_and_write_commands(
       }
 
       double error = reference_interfaces_[i] - measured_state_values_[i];
+
+      const auto zero_threshold = params_.gains.dof_names_map[params_.dof_names[i]].zero_threshold;
+      if (std::abs(reference_interfaces_[i]) < zero_threshold && std::abs(error) < zero_threshold)
+      {
+        reset_pid_time_ += period.seconds();
+        if (
+          reset_pid_time_ >
+          params_.gains.dof_names_map[params_.dof_names[i]].zero_threshold_duration)
+        {
+          pids_[i]->reset();
+        }
+      }
+      else
+      {
+        reset_pid_time_ = 0.0;
+      }
+
       if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound)
       {
         // for continuous angles the error is normalized between -pi<error<pi

pid_controller/src/pid_controller.yaml

@@ -94,3 +94,17 @@ pid_controller:
         default_value: true,
         description: "Indicating if integral term is retained after re-activation"
       }
+      zero_threshold: {
+        type: double,
+        default_value: 0.0,
+        description: "If the output is below this threshold for a duration specified by
+        zero_threshold_duration, the PID will be reset. If set to 0.0, this functionality is
+        disabled. To reset integral term, set the save_i_term parameter to false."
+      }
+      zero_threshold_duration: {
+        type: double,
+        default_value: 0.5,
+        description: "Duration in seconds for which the output must be below the zero_threshold
+        before the PID will be reset. This is used to prevent the PID from resetting when the
+        output is only briefly below the threshold."
+      }

pid_controller/test/pid_controller_params.yaml

@@ -71,3 +71,15 @@ test_save_i_term_on:
 
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true}
+
+test_zero_threshold:
+  ros__parameters:
+    dof_names:
+      - joint1
+
+    command_interface: position
+
+    reference_and_state_interfaces: ["position"]
+
+    gains:
+      joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false, zero_threshold: 0.1, zero_threshold_duration: 0.01}

pid_controller/test/test_pid_controller.cpp

@@ -744,6 +744,59 @@ TEST_F(PidControllerTest, test_save_i_term_on)
   EXPECT_NEAR(actual_value, 2.00002, 1e-5);  // i_term from above
 }
 
+/**
+ * @brief Test if zero_threshold is working
+ *
+ */
+
+TEST_F(PidControllerTest, test_zero_threshold)
+{
+  SetUpController("test_zero_threshold");
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+  executor.add_node(service_caller_node_->get_node_base_interface());
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  controller_->set_chained_mode(false);
+  for (const auto & dof_name : dof_names_)
+  {
+    ASSERT_NE(controller_->params_.gains.dof_names_map[dof_name].zero_threshold, 0.0);
+  }
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_FALSE(controller_->is_in_chained_mode());
+
+  controller_->set_reference(dof_command_values_);
+
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // check the command value
+  // error = ref - state = 100.001, error_dot = error/ds = 10000.1,
+  // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3
+  // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3
+  const double expected_command_value = 30102.30102;
+
+  double actual_value =
+    std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5;
+  EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
+
+  // set command and state to a value slightly less than zero threshold
+  const auto near_zero_value =
+    controller_->params_.gains.dof_names_map[dof_names_[0]].zero_threshold * 0.9;
+  controller_->set_reference({near_zero_value});
+  dof_state_values_[0] = near_zero_value;
+
+  // set the duration to a value larger than the zero threshold duration
+  const auto duration =
+    controller_->params_.gains.dof_names_map[dof_names_[0]].zero_threshold_duration * 1.1;
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(duration)),
+    controller_interface::return_type::OK);
+  actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5;
+  EXPECT_NEAR(actual_value, 0.0, 1e-5);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

pid_controller/test/test_pid_controller.hpp

@@ -66,6 +66,7 @@ class TestablePidController : public pid_controller::PidController
   FRIEND_TEST(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface);
   FRIEND_TEST(PidControllerTest, test_save_i_term_on);
   FRIEND_TEST(PidControllerTest, test_save_i_term_off);
+  FRIEND_TEST(PidControllerTest, test_zero_threshold);
 
 public:
   controller_interface::CallbackReturn on_configure(