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/8] 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/8] 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/8] 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) { From b40040761da18fa14470bbae12a5a2527df927ee Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Tue, 4 Feb 2025 08:31:18 +0100 Subject: [PATCH 4/8] fix: adds missing `pos_ind +` --- admittance_controller/src/admittance_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index b3bbb09c5f..f49e3f832c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -559,7 +559,8 @@ 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 = (has_position_command_interface_) ? has_velocity_command_interface_ : pos_ind; + size_t vel_ind = + (has_position_command_interface_) ? pos_ind + 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) { From b1fd7a9e03bbe3fe4ea554c7cb1553b0e9fdb038 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Tue, 11 Feb 2025 11:04:05 +0100 Subject: [PATCH 5/8] AdmittanceController: adds tests to check - configuration failed if position state interface is not provided - compatibility with only velocity command interface --- .../test/test_admittance_controller.cpp | 19 +++++++++++++++++++ .../test/test_admittance_controller.hpp | 5 ++++- 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 5c4bbe9438..5c0af74c13 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -180,6 +180,25 @@ TEST_F(AdmittanceControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); } +TEST_F(AdmittanceControllerTest, missing_pos_state_interface) +{ + auto overrides = {rclcpp::Parameter("state_interfaces", std::vector{"velocity"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); +} + +TEST_F(AdmittanceControllerTest, only_vel_command_interface) +{ + command_interface_types_ = {"velocity"}; + auto overrides = {rclcpp::Parameter("command_interfaces", std::vector{"velocity"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + TEST_F(AdmittanceControllerTest, update_success) { SetUpController(); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 7ee56b8c11..364bea1775 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -56,6 +56,9 @@ constexpr auto NODE_FAILURE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + +constexpr auto NODE_CONFIGURE = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } // namespace // subclassing and friending so we can access member variables @@ -354,7 +357,7 @@ class AdmittanceControllerTest : public ::testing::Test // Controller-related parameters const std::vector joint_names_ = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; - const std::vector command_interface_types_ = {"position"}; + std::vector command_interface_types_ = {"position"}; const std::vector state_interface_types_ = {"position"}; const std::string ft_sensor_name_ = "ft_sensor_name"; From e13863cd254741063083e6684bcf94e648289886 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Tue, 11 Feb 2025 11:33:38 +0100 Subject: [PATCH 6/8] AdmittanceController: allow to export position and velocity reference interface independently --- .../admittance_controller.hpp | 2 - .../src/admittance_controller.cpp | 61 +++++++++++++------ .../test/test_admittance_controller.cpp | 24 ++++++++ .../test/test_admittance_controller.hpp | 3 - 4 files changed, 66 insertions(+), 24 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 8f0bc25973..23aad881c6 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -106,8 +106,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt hardware_interface::HW_IF_ACCELERATION}; // internal reference values - const std::vector allowed_reference_interfaces_types_ = { - hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}; std::vector> position_reference_; std::vector> velocity_reference_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index f49e3f832c..e95c5e2644 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -137,7 +137,7 @@ AdmittanceController::on_export_reference_interfaces() // assign reference interfaces auto index = 0ul; - for (const auto & interface : allowed_reference_interfaces_types_) + for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) { for (const auto & joint : admittance_->parameters_.joints) { @@ -412,13 +412,22 @@ controller_interface::return_type AdmittanceController::update_reference_from_su // if message exists, load values into references if (joint_command_msg_.get()) { - for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) { - position_reference_[i].get() = joint_command_msg_->positions[i]; - } - for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) - { - velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + if (interface == hardware_interface::HW_IF_POSITION) + { + for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + { + position_reference_[i].get() = joint_command_msg_->positions[i]; + } + } + else if (interface == hardware_interface::HW_IF_VELOCITY) + { + for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) + { + velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + } + } } } @@ -470,8 +479,13 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( // reset to prevent stale references for (size_t i = 0; i < num_joints_; i++) { - position_reference_[i].get() = std::numeric_limits::quiet_NaN(); - velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) + { + if (interface == hardware_interface::HW_IF_POSITION) + position_reference_[i].get() = std::numeric_limits::quiet_NaN(); + else if (interface == hardware_interface::HW_IF_VELOCITY) + velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + } } for (size_t index = 0; index < allowed_interface_types_.size(); ++index) @@ -591,19 +605,28 @@ void AdmittanceController::read_state_reference_interfaces( // if any interface has nan values, assume state_reference is the last set reference for (size_t i = 0; i < num_joints_; ++i) { - // update position - if (std::isnan(position_reference_[i])) + for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) { - position_reference_[i].get() = last_reference_.positions[i]; - } - state_reference.positions[i] = position_reference_[i]; + // update position + if (interface == hardware_interface::HW_IF_POSITION) + { + if (std::isnan(position_reference_[i])) + { + position_reference_[i].get() = last_reference_.positions[i]; + } + state_reference.positions[i] = position_reference_[i]; + } - // update velocity - if (std::isnan(velocity_reference_[i])) - { - velocity_reference_[i].get() = last_reference_.velocities[i]; + // update velocity + if (interface == hardware_interface::HW_IF_VELOCITY) + { + if (std::isnan(velocity_reference_[i])) + { + velocity_reference_[i].get() = last_reference_.velocities[i]; + } + state_reference.velocities[i] = velocity_reference_[i]; + } } - state_reference.velocities[i] = velocity_reference_[i]; } last_reference_.positions = state_reference.positions; diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 5c0af74c13..792490061e 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -199,6 +199,30 @@ TEST_F(AdmittanceControllerTest, only_vel_command_interface) controller_interface::return_type::OK); } +TEST_F(AdmittanceControllerTest, only_pos_reference_interface) +{ + auto overrides = { + rclcpp::Parameter("chainable_command_interfaces", std::vector{"position"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, only_vel_reference_interface) +{ + auto overrides = { + rclcpp::Parameter("chainable_command_interfaces", std::vector{"velocity"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, invalid_reference_interface) +{ + auto overrides = {rclcpp::Parameter( + "chainable_command_interfaces", std::vector{"invalid_interface"})}; + SetUpController("test_admittance_controller", overrides); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + TEST_F(AdmittanceControllerTest, update_success) { SetUpController(); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 364bea1775..78ddf8d46d 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -56,9 +56,6 @@ constexpr auto NODE_FAILURE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - -constexpr auto NODE_CONFIGURE = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } // namespace // subclassing and friending so we can access member variables From d651d88e3005be19df06de1525cbd6ea593722b8 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 08:20:48 +0100 Subject: [PATCH 7/8] AdmittanceController: fixes default value of `chainable_command_interfaces` to position and velocity to ensure same default behavior after `allowed_reference_interfaces_types_` removal --- admittance_controller/src/admittance_controller_parameters.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 315d0e70d2..c0c0beb9e5 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -27,6 +27,7 @@ admittance_controller: chainable_command_interfaces: { type: string_array, + default_value: ["position", "velocity"], description: "Specifies which reference interfaces the controller will export. Normally, the position and velocity are used.", read_only: true } From 1fc99c4c5b1f9fed47647dcc481f0912dbcc5cc6 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Thu, 13 Feb 2025 07:50:35 +0100 Subject: [PATCH 8/8] AdmittanceController: removes `chainable_command_interfaces` from tested missing parameters after default value addition --- admittance_controller/test/test_admittance_controller.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 792490061e..8105632c19 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -28,9 +28,8 @@ TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_init_paramete INSTANTIATE_TEST_SUITE_P( MissingMandatoryParameterDuringInit, AdmittanceControllerTestParameterizedMissingParameters, ::testing::Values( - "admittance.mass", "admittance.selected_axes", "admittance.stiffness", - "chainable_command_interfaces", "command_interfaces", "control.frame.id", - "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", + "admittance.mass", "admittance.selected_axes", "admittance.stiffness", "command_interfaces", + "control.frame.id", "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces"));