Fixed interface indexing issue

ros-controls · Dec 21, 2024 · 2899780 · 2899780
1 parent a3f5e2b
commit 2899780
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp
@@ -520,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)
   {
@@ -574,7 +574,7 @@ 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 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_)