fix: uses correct has flags in read/write methods

ros-controls · Jul 6, 2024 · f427c4d · f427c4d
1 parent 10ad1c3
commit f427c4d
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
@@ -470,7 +470,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)
   {
@@ -524,7 +524,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_)