diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index a4f33bacd2..8c9d7dab5c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -89,7 +89,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt protected: std::vector export_state_interface_descriptions() override; - std::vector export_reference_interface_descriptions() + std::vector export_reference_interface_descriptions() override; controller_interface::return_type update_reference_from_subscribers( @@ -124,8 +124,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // 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_; + std::vector position_reference_names_; + std::vector velocity_reference_names_; // Admittance rule and dependent variables; std::unique_ptr admittance_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e29b574a2..788a07c240 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -97,8 +97,14 @@ controller_interface::InterfaceConfiguration AdmittanceController::state_interfa controller_interface::interface_configuration_type::INDIVIDUAL, state_interfaces_config_names}; } -std::vector -AdmittanceController::on_export_reference_interfaces() +std::vector +AdmittanceController::export_state_interface_descriptions() +{ + return {}; +} + +std::vector +AdmittanceController::export_reference_interface_descriptions() { // create CommandInterface interfaces that other controllers will be able to chain with if (!admittance_) @@ -106,34 +112,32 @@ AdmittanceController::on_export_reference_interfaces() return {}; } - std::vector chainable_command_interfaces; + std::vector chainable_command_interfaces; const auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * admittance_->parameters_.joints.size(); // allocate dynamic memory chainable_command_interfaces.reserve(num_chainable_interfaces); - reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); - position_reference_ = {}; - velocity_reference_ = {}; + position_reference_names_ = {}; + velocity_reference_names_ = {}; - // assign reference interfaces - auto index = 0ul; + const auto prefix = std::string(get_node()->get_name()); + // create reference interfaces for (const auto & interface : allowed_reference_interfaces_types_) { for (const auto & joint : admittance_->parameters_.joints) { + const auto itf_name = joint + "/" + interface; + chainable_command_interfaces.emplace_back( + prefix, hardware_interface::InterfaceInfo(itf_name, "double")); + // add reference interface names in order to position or velocity if (hardware_interface::HW_IF_POSITION == interface) - position_reference_.emplace_back(reference_interfaces_[index]); + position_reference_names_.push_back(chainable_command_interfaces.back().get_name()); else if (hardware_interface::HW_IF_VELOCITY == interface) { - velocity_reference_.emplace_back(reference_interfaces_[index]); + velocity_reference_names_.push_back(chainable_command_interfaces.back().get_name()); } - const auto full_name = joint + "/" + interface; - chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( - std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); - - index++; } } @@ -370,11 +374,13 @@ controller_interface::return_type AdmittanceController::update_reference_from_su { for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) { - position_reference_[i].get() = joint_command_msg_->positions[i]; + reference_interfaces_.at(position_reference_names_[i]) + ->set_value(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]; + reference_interfaces_.at(velocity_reference_names_[i]) + ->set_value(joint_command_msg_->velocities[i]); } } @@ -424,8 +430,10 @@ 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(); + reference_interfaces_.at(position_reference_names_[i]) + ->set_value(std::numeric_limits::quiet_NaN()); + reference_interfaces_.at(velocity_reference_names_[i]) + ->set_value(std::numeric_limits::quiet_NaN()); } for (size_t index = 0; index < allowed_interface_types_.size(); ++index) @@ -473,19 +481,19 @@ void AdmittanceController::read_state_from_hardware( if (has_position_state_interface_) { state_current.positions[joint_ind] = - state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); + 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_) { state_current.velocities[joint_ind] = - state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); + 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_) { state_current.accelerations[joint_ind] = - state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); + state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]); } } @@ -551,18 +559,22 @@ void AdmittanceController::read_state_reference_interfaces( for (size_t i = 0; i < num_joints_; ++i) { // update position - if (std::isnan(position_reference_[i])) + if (std::isnan(reference_interfaces_.at(position_reference_names_[i])->get_value())) { - position_reference_[i].get() = last_reference_.positions[i]; + reference_interfaces_.at(position_reference_names_[i]) + ->set_value(last_reference_.positions[i]); } - state_reference.positions[i] = position_reference_[i]; + state_reference.positions[i] = + reference_interfaces_.at(position_reference_names_[i])->get_value(); // update velocity - if (std::isnan(velocity_reference_[i])) + if (std::isnan(reference_interfaces_.at(velocity_reference_names_[i])->get_value())) { - velocity_reference_[i].get() = last_reference_.velocities[i]; + reference_interfaces_.at(velocity_reference_names_[i]) + ->set_value(last_reference_.velocities[i]); } - state_reference.velocities[i] = velocity_reference_[i]; + state_reference.velocities[i] = + reference_interfaces_.at(velocity_reference_names_[i])->get_value(); } 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 5c4bbe9438..bda676e591 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -154,7 +154,7 @@ TEST_F(AdmittanceControllerTest, check_interfaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto command_interfaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + ASSERT_EQ(command_interfaces.names.size(), initial_joint_command_values_.size()); EXPECT_EQ( command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); @@ -162,12 +162,14 @@ TEST_F(AdmittanceControllerTest, check_interfaces) controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); auto state_interfaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + ASSERT_EQ( + state_interfaces.names.size(), + initial_joint_state_values_.size() + initial_fts_state_values_.size()); EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->state_interfaces_.size(), - state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); + state_interface_types_.size() * joint_names_.size() + initial_fts_state_values_.size()); } TEST_F(AdmittanceControllerTest, activate_success) @@ -264,9 +266,10 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) controller_interface::return_type::OK); // After first update state, commanded position should be near the start state - for (auto i = 0ul; i < joint_state_values_.size(); i++) + for (auto i = 0ul; i < state_itfs_.size(); i++) { - ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); + ASSERT_NEAR( + state_itfs_[i].get_value(), command_itfs_[i].get_value(), COMMON_THRESHOLD); } ControllerStateMsg msg; @@ -282,7 +285,8 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); + EXPECT_NEAR( + command_itfs_[0].get_value(), state_itfs_[0].get_value(), COMMON_THRESHOLD); subscribe_and_get_messages(msg); } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index b2a95c12fa..8cfbeb86e7 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -174,13 +174,15 @@ class AdmittanceControllerTest : public ::testing::Test void assign_interfaces() { std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + command_itfs_.reserve(initial_joint_command_values_.size()); + command_ifs.reserve(initial_joint_command_values_.size()); - for (auto i = 0u; i < joint_command_values_.size(); ++i) + for (auto i = 0u; i < initial_joint_command_values_.size(); ++i) { - command_itfs_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + command_itfs_.emplace_back(hardware_interface::InterfaceDescription( + joint_names_[i], hardware_interface::InterfaceInfo( + command_interface_types_[0], "double", + std::to_string(initial_joint_command_values_[i])))); command_ifs.emplace_back(command_itfs_.back()); } @@ -188,14 +190,16 @@ class AdmittanceControllerTest : public ::testing::Test fts_state_names_ = sc_fts.get_state_interface_names(); std::vector state_ifs; - const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size(); + const size_t num_state_ifs = initial_joint_state_values_.size() + fts_state_names_.size(); state_itfs_.reserve(num_state_ifs); state_ifs.reserve(num_state_ifs); - for (auto i = 0u; i < joint_state_values_.size(); ++i) + for (auto i = 0u; i < initial_joint_state_values_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + state_itfs_.emplace_back(hardware_interface::InterfaceDescription( + joint_names_[i], + hardware_interface::InterfaceInfo( + state_interface_types_[0], std::to_string(initial_joint_command_values_[i])))); state_ifs.emplace_back(state_itfs_.back()); } @@ -204,8 +208,10 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < fts_state_names_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( - ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); + state_itfs_.emplace_back(hardware_interface::InterfaceDescription( + ft_sensor_name_, + hardware_interface::InterfaceInfo( + fts_itf_names[i], "double", std::to_string(initial_fts_state_values_[i])))); state_ifs.emplace_back(state_itfs_.back()); } @@ -344,7 +350,7 @@ class AdmittanceControllerTest : public ::testing::Test trajectory_point.velocities.resize(num_joints, 0.0); for (auto index = 0u; index < num_joints; ++index) { - trajectory_point.positions.emplace_back(joint_state_values_[index]); + trajectory_point.positions.emplace_back(initial_joint_state_values_[index]); } joint_msg = trajectory_point; @@ -380,9 +386,9 @@ class AdmittanceControllerTest : public ::testing::Test 2.828427, 2.828427, 2.828427}; std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; - std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; - std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + const std::array initial_joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + const std::array initial_joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + const std::array initial_fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector fts_state_names_; std::vector state_itfs_;