From 44b901d2cd115b720ac1df6f9ca568d7860769df Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Wed, 31 Jul 2024 12:48:58 +0200 Subject: [PATCH 1/2] write correct state to command interfaces --- 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 6e29b574a2..2601d10398 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -531,12 +531,12 @@ void AdmittanceController::write_state_to_hardware( else 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_) { command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.accelerations[joint_ind]); } } last_commanded_ = state_commanded; From ffc3c748d7fb2c587ee3bc574e16c319107fefd4 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Wed, 31 Jul 2024 12:49:39 +0200 Subject: [PATCH 2/2] in read/write, replace if-else clauses with if clauses --- admittance_controller/src/admittance_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 2601d10398..a4b56d739c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -476,13 +476,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(); @@ -528,12 +528,12 @@ 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.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.accelerations[joint_ind]);