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 1ef76d1f61..b4c0586473 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) { @@ -258,6 +258,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; @@ -406,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]; + } + } } } @@ -464,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) @@ -500,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) { @@ -553,8 +573,9 @@ 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 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) { if (has_position_command_interface_) @@ -584,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/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 } diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index f6e28d599e..f26cd4df22 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")); @@ -201,6 +200,49 @@ 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, 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 7ee56b8c11..78ddf8d46d 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -354,7 +354,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";