Fix admittance controller interface read/write logic (backport #1232) (…

…#1234)
ros-controls · Aug 1, 2024 · 60010c6 · 60010c6
1 parent 66df39f
commit 60010c6
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 7 deletions.
diff --git 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
@@ -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