[pid_controller] Fix logic for feedforward_mode with single reference…

… interface (#1520)

(cherry picked from commit cf8b96e)

pid_controller/src/pid_controller.cpp

@@ -433,20 +433,27 @@ controller_interface::return_type PidController::update_and_write_commands(
 
   for (size_t i = 0; i < dof_; ++i)
   {
-    double tmp_command = std::numeric_limits<double>::quiet_NaN();
+    double tmp_command = 0.0;
 
-    // Using feedforward
     if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i]))
     {
       // calculate feed-forward
       if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON)
       {
-        tmp_command = reference_interfaces_[dof_ + i] *
-                      params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain;
-      }
-      else
-      {
-        tmp_command = 0.0;
+        // two interfaces
+        if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
+        {
+          if (std::isfinite(reference_interfaces_[dof_ + i]))
+          {
+            tmp_command = reference_interfaces_[dof_ + i] *
+                          params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain;
+          }
+        }
+        else  // one interface
+        {
+          tmp_command = reference_interfaces_[i] *
+                        params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain;
+        }
       }
 
       double error = reference_interfaces_[i] - measured_state_values_[i];

pid_controller/test/pid_controller_params.yaml

@@ -9,3 +9,16 @@ test_pid_controller:
 
     gains:
       joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0}
+
+
+test_pid_controller_with_feedforward_gain:
+  ros__parameters:
+    dof_names:
+      - joint1
+
+    command_interface: position
+
+    reference_and_state_interfaces: ["position"]
+
+    gains:
+      joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0}

pid_controller/test/test_pid_controller.cpp

@@ -522,6 +522,137 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status)
   }
 }
 
+/**
+ * @brief check chained pid controller with feedforward and gain as non-zero, single interface
+ */
+TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain)
+{
+  // state interface value is 1.1 as defined in test fixture
+  // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95
+  // with feedforward gain 1.0, the command value should be 1.95 + 1.0 * 5.0 = 6.95
+  const double target_value = 5.0;
+  const double expected_command_value = 6.95;
+
+  SetUpController("test_pid_controller_with_feedforward_gain");
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check on interfaces & pid gain parameters
+  for (const auto & dof_name : dof_names_)
+  {
+    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5);
+    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0);
+  }
+  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);
+
+  // setup executor
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+  executor.add_node(service_caller_node_->get_node_base_interface());
+
+  controller_->set_chained_mode(true);
+
+  // activate controller
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_TRUE(controller_->is_in_chained_mode());
+
+  // turn on feedforward
+  controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON);
+  ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
+
+  // send a message to update reference interface
+  std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
+  msg->dof_names = controller_->params_.dof_names;
+  msg->values.resize(msg->dof_names.size(), 0.0);
+  for (size_t i = 0; i < msg->dof_names.size(); ++i)
+  {
+    msg->values[i] = target_value;
+  }
+  msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits<double>::quiet_NaN());
+  controller_->input_ref_.writeFromNonRT(msg);
+  ASSERT_EQ(
+    controller_->update_reference_from_subscribers(
+      rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // run update
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // check on result from update
+  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
+}
+
+/**
+ * @brief check chained pid controller with feedforward OFF and gain as non-zero, single interface
+ */
+TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain)
+{
+  // state interface value is 1.1 as defined in test fixture
+  // given target value 5.0
+  // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95
+  // with feedforward off, the command value should be still 1.95 even though feedforward gain
+  // is 1.0
+  const double target_value = 5.0;
+  const double expected_command_value = 1.95;
+
+  SetUpController("test_pid_controller_with_feedforward_gain");
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // check on interfaces & pid gain parameters
+  for (const auto & dof_name : dof_names_)
+  {
+    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5);
+    ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0);
+  }
+  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);
+
+  // setup executor
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+  executor.add_node(service_caller_node_->get_node_base_interface());
+
+  controller_->set_chained_mode(true);
+
+  // activate controller
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_TRUE(controller_->is_in_chained_mode());
+
+  // feedforward by default is OFF
+  ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
+
+  // send a message to update reference interface
+  std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
+  msg->dof_names = controller_->params_.dof_names;
+  msg->values.resize(msg->dof_names.size(), 0.0);
+  for (size_t i = 0; i < msg->dof_names.size(); ++i)
+  {
+    msg->values[i] = target_value;
+  }
+  msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits<double>::quiet_NaN());
+  controller_->input_ref_.writeFromNonRT(msg);
+  ASSERT_EQ(
+    controller_->update_reference_from_subscribers(
+      rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // run update
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // check on result from update
+  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

pid_controller/test/test_pid_controller.hpp

@@ -60,6 +60,8 @@ class TestablePidController : public pid_controller::PidController
   FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on);
   FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success);
   FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status);
+  FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain);
+  FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain);
 
 public:
   controller_interface::CallbackReturn on_configure(