AdmittanceController: allow to export position and velocity reference…

… interface independently
ros-controls · Feb 11, 2025 · e13863c · e13863c
1 parent b1fd7a9
commit e13863c
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 24 deletions.
diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/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_;
 

diff --git a/admittance_controller/src/admittance_controller.cpp b/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)
     {
@@ -412,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];
+        }
+      }
     }
   }
 
@@ -470,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)
@@ -591,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;

diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp
@@ -199,6 +199,30 @@ TEST_F(AdmittanceControllerTest, only_vel_command_interface)
     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();

diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp
@@ -56,9 +56,6 @@ constexpr auto NODE_FAILURE =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
 constexpr auto NODE_ERROR =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
-
-constexpr auto NODE_CONFIGURE =
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
 }  // namespace
 
 // subclassing and friending so we can access member variables