[Admittance] multiple state/command interfaces

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -106,8 +106,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
     hardware_interface::HW_IF_ACCELERATION};
 
   // internal reference values
-  const std::vector<std::string> allowed_reference_interfaces_types_ = {
-    hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY};
   std::vector<std::reference_wrapper<double>> position_reference_;
   std::vector<std::reference_wrapper<double>> velocity_reference_;
 

admittance_controller/src/admittance_controller.cpp

@@ -137,7 +137,7 @@ AdmittanceController::on_export_reference_interfaces()
 
   // assign reference interfaces
   auto index = 0ul;
-  for (const auto & interface : allowed_reference_interfaces_types_)
+  for (const auto & interface : admittance_->parameters_.chainable_command_interfaces)
   {
     for (const auto & joint : admittance_->parameters_.joints)
     {
@@ -258,6 +258,12 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
   has_acceleration_state_interface_ = contains_interface_type(
     admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_ACCELERATION);
 
+  if (!has_position_state_interface_)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Position state interface is required.");
+    return CallbackReturn::FAILURE;
+  }
+
   auto get_interface_list = [](const std::vector<std::string> & interface_types)
   {
     std::stringstream ss_command_interfaces;
@@ -406,13 +412,22 @@ controller_interface::return_type AdmittanceController::update_reference_from_su
   // if message exists, load values into references
   if (joint_command_msg_.get())
   {
-    for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i)
+    for (const auto & interface : admittance_->parameters_.chainable_command_interfaces)
     {
-      position_reference_[i].get() = joint_command_msg_->positions[i];
-    }
-    for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i)
-    {
-      velocity_reference_[i].get() = joint_command_msg_->velocities[i];
+      if (interface == hardware_interface::HW_IF_POSITION)
+      {
+        for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i)
+        {
+          position_reference_[i].get() = joint_command_msg_->positions[i];
+        }
+      }
+      else if (interface == hardware_interface::HW_IF_VELOCITY)
+      {
+        for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i)
+        {
+          velocity_reference_[i].get() = joint_command_msg_->velocities[i];
+        }
+      }
     }
   }
 
@@ -464,8 +479,13 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate(
   // reset to prevent stale references
   for (size_t i = 0; i < num_joints_; i++)
   {
-    position_reference_[i].get() = std::numeric_limits<double>::quiet_NaN();
-    velocity_reference_[i].get() = std::numeric_limits<double>::quiet_NaN();
+    for (const auto & interface : admittance_->parameters_.chainable_command_interfaces)
+    {
+      if (interface == hardware_interface::HW_IF_POSITION)
+        position_reference_[i].get() = std::numeric_limits<double>::quiet_NaN();
+      else if (interface == hardware_interface::HW_IF_VELOCITY)
+        velocity_reference_[i].get() = std::numeric_limits<double>::quiet_NaN();
+    }
   }
 
   for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
@@ -500,7 +520,7 @@ void AdmittanceController::read_state_from_hardware(
   bool nan_acceleration = false;
 
   size_t pos_ind = 0;
-  size_t vel_ind = pos_ind + has_velocity_command_interface_;
+  size_t vel_ind = pos_ind + has_velocity_state_interface_;
   size_t acc_ind = vel_ind + has_acceleration_state_interface_;
   for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind)
   {
@@ -553,8 +573,9 @@ void AdmittanceController::write_state_to_hardware(
 {
   // if any interface has nan values, assume state_commanded is the last command state
   size_t pos_ind = 0;
-  size_t vel_ind = pos_ind + has_velocity_command_interface_;
-  size_t acc_ind = vel_ind + has_acceleration_state_interface_;
+  size_t vel_ind =
+    (has_position_command_interface_) ? pos_ind + has_velocity_command_interface_ : pos_ind;
+  size_t acc_ind = vel_ind + has_acceleration_command_interface_;
   for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind)
   {
     if (has_position_command_interface_)
@@ -584,19 +605,28 @@ void AdmittanceController::read_state_reference_interfaces(
   // if any interface has nan values, assume state_reference is the last set reference
   for (size_t i = 0; i < num_joints_; ++i)
   {
-    // update position
-    if (std::isnan(position_reference_[i]))
+    for (const auto & interface : admittance_->parameters_.chainable_command_interfaces)
     {
-      position_reference_[i].get() = last_reference_.positions[i];
-    }
-    state_reference.positions[i] = position_reference_[i];
+      // update position
+      if (interface == hardware_interface::HW_IF_POSITION)
+      {
+        if (std::isnan(position_reference_[i]))
+        {
+          position_reference_[i].get() = last_reference_.positions[i];
+        }
+        state_reference.positions[i] = position_reference_[i];
+      }
 
-    // update velocity
-    if (std::isnan(velocity_reference_[i]))
-    {
-      velocity_reference_[i].get() = last_reference_.velocities[i];
+      // update velocity
+      if (interface == hardware_interface::HW_IF_VELOCITY)
+      {
+        if (std::isnan(velocity_reference_[i]))
+        {
+          velocity_reference_[i].get() = last_reference_.velocities[i];
+        }
+        state_reference.velocities[i] = velocity_reference_[i];
+      }
     }
-    state_reference.velocities[i] = velocity_reference_[i];
   }
 
   last_reference_.positions = state_reference.positions;

admittance_controller/test/test_admittance_controller.cpp

@@ -180,6 +180,49 @@ TEST_F(AdmittanceControllerTest, activate_success)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
 }
 
+TEST_F(AdmittanceControllerTest, missing_pos_state_interface)
+{
+  auto overrides = {rclcpp::Parameter("state_interfaces", std::vector<std::string>{"velocity"})};
+  SetUpController("test_admittance_controller", overrides);
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE);
+}
+
+TEST_F(AdmittanceControllerTest, only_vel_command_interface)
+{
+  command_interface_types_ = {"velocity"};
+  auto overrides = {rclcpp::Parameter("command_interfaces", std::vector<std::string>{"velocity"})};
+  SetUpController("test_admittance_controller", overrides);
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(
+    controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+}
+
+TEST_F(AdmittanceControllerTest, only_pos_reference_interface)
+{
+  auto overrides = {
+    rclcpp::Parameter("chainable_command_interfaces", std::vector<std::string>{"position"})};
+  SetUpController("test_admittance_controller", overrides);
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+}
+
+TEST_F(AdmittanceControllerTest, only_vel_reference_interface)
+{
+  auto overrides = {
+    rclcpp::Parameter("chainable_command_interfaces", std::vector<std::string>{"velocity"})};
+  SetUpController("test_admittance_controller", overrides);
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+}
+
+TEST_F(AdmittanceControllerTest, invalid_reference_interface)
+{
+  auto overrides = {rclcpp::Parameter(
+    "chainable_command_interfaces", std::vector<std::string>{"invalid_interface"})};
+  SetUpController("test_admittance_controller", overrides);
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR);
+}
+
 TEST_F(AdmittanceControllerTest, update_success)
 {
   SetUpController();

admittance_controller/test/test_admittance_controller.hpp

@@ -354,7 +354,7 @@ class AdmittanceControllerTest : public ::testing::Test
   // Controller-related parameters
   const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3",
                                                  "joint4", "joint5", "joint6"};
-  const std::vector<std::string> command_interface_types_ = {"position"};
+  std::vector<std::string> command_interface_types_ = {"position"};
   const std::vector<std::string> state_interface_types_ = {"position"};
   const std::string ft_sensor_name_ = "ft_sensor_name";