From 28997804650c2ab31543bc09d34917a3807820e3 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 20 Dec 2024 16:32:06 -0600 Subject: [PATCH] Fixed interface indexing issue --- admittance_controller/src/admittance_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e0c34a35c..a7ae6853e8 100644 --- 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_)