From 10ad1c38366cfe78fbea0eca24768e8af7959bf7 Mon Sep 17 00:00:00 2001 From: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com> Date: Sat, 6 Jul 2024 09:37:00 +0200 Subject: [PATCH 1/3] fix: read/writes all configured state/command interfaces. Adjust state_command usage in `write_state_to_hardware` method --- admittance_controller/src/admittance_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c6a8168736..22d25bb8b1 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -480,13 +480,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(); @@ -532,15 +532,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; From f427c4da2be4690d29fde71185f290e28a8c767e Mon Sep 17 00:00:00 2001 From: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com> Date: Sat, 6 Jul 2024 09:39:01 +0200 Subject: [PATCH 2/3] fix: uses correct `has` flags in read/write methods --- 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 22d25bb8b1..9445ec1733 100644 --- 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_) From 7ac93f6bce00247f24e3c4b0dbd798ac0db6ef3b Mon Sep 17 00:00:00 2001 From: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com> Date: Sat, 6 Jul 2024 10:02:50 +0200 Subject: [PATCH 3/3] fix: makes position state interface mandatory, adds compatibility with velocity command interface without position command interface --- admittance_controller/src/admittance_controller.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 9445ec1733..c328ea5d89 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -244,6 +244,12 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( has_acceleration_state_interface_ = contains_interface_type( admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); + if (!has_position_state_interface_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Position state interface is required."); + return CallbackReturn::FAILURE; + } + auto get_interface_list = [](const std::vector & interface_types) { std::stringstream ss_command_interfaces; @@ -523,7 +529,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 vel_ind = (has_position_command_interface_) ? has_velocity_command_interface_ : pos_ind; size_t acc_ind = vel_ind + has_acceleration_command_interface_; for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) {