From 60010c625fd18da317f1f00eb324ac6a909942c6 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Thu, 1 Aug 2024 09:13:25 +0100
Subject: [PATCH] Fix admittance controller interface read/write logic
 (backport #1232) (#1234)

---
 admittance_controller/src/admittance_controller.cpp  | 12 ++++++------
 .../test/test_trajectory_controller_utils.hpp        |  1 -
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp
index 1af4e1317f..0833a9f9f3 100644
--- a/admittance_controller/src/admittance_controller.cpp
+++ b/admittance_controller/src/admittance_controller.cpp
@@ -491,13 +491,13 @@ void AdmittanceController::read_state_from_hardware(
         state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value();
       nan_position |= std::isnan(state_current.positions[joint_ind]);
     }
-    else if (has_velocity_state_interface_)
+    if (has_velocity_state_interface_)
     {
       state_current.velocities[joint_ind] =
         state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value();
       nan_velocity |= std::isnan(state_current.velocities[joint_ind]);
     }
-    else if (has_acceleration_state_interface_)
+    if (has_acceleration_state_interface_)
     {
       state_current.accelerations[joint_ind] =
         state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value();
@@ -543,15 +543,15 @@ void AdmittanceController::write_state_to_hardware(
       command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
         state_commanded.positions[joint_ind]);
     }
-    else if (has_velocity_command_interface_)
+    if (has_velocity_command_interface_)
     {
       command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
-        state_commanded.positions[joint_ind]);
+        state_commanded.velocities[joint_ind]);
     }
-    else if (has_acceleration_command_interface_)
+    if (has_acceleration_command_interface_)
     {
       command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
-        state_commanded.positions[joint_ind]);
+        state_commanded.accelerations[joint_ind]);
     }
   }
   last_commanded_ = state_commanded;
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 78ef8d120b..5891f72f82 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -187,7 +187,6 @@ class TestableJointTrajectoryController
   trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; }
   trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; }
   trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; }
-
 };
 
 class TrajectoryControllerTest : public ::testing::Test