From 34627637ea2bbe9d0c7b547c5b8f4918dfd811bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 6 May 2021 10:16:36 +0200 Subject: [PATCH] Testable version of Admittance Controller. IK not yet enables, so there is not output. --- admittance_controller/CMakeLists.txt | 58 ++- .../admittance_controller.hpp | 73 +++- .../admittance_controller/admittance_rule.hpp | 162 ++++++++ .../incremental_ik_calculator.hpp | 3 +- admittance_controller/package.xml | 11 +- .../src/admittance_controller.cpp | 379 ++++++++++++++--- admittance_controller/src/admittance_rule.cpp | 286 +++++++++++++ .../src/incremental_ik_calculator.cpp | 2 + .../test/test_admittance_controller.cpp | 368 +++++++++++++++++ .../test/test_admittance_controller.hpp | 389 ++++++++++++++++++ doc/admittance_controller.rst | 58 +++ 11 files changed, 1710 insertions(+), 79 deletions(-) create mode 100644 admittance_controller/include/admittance_controller/admittance_rule.hpp create mode 100644 admittance_controller/src/admittance_rule.cpp create mode 100644 admittance_controller/test/test_admittance_controller.cpp create mode 100644 admittance_controller/test/test_admittance_controller.hpp create mode 100644 doc/admittance_controller.rst diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index b7a72ba779..797942911c 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -11,37 +11,55 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(hardware_interface REQUIRED) +find_package(iirob_filters REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) add_library( admittance_controller SHARED src/admittance_controller.cpp + src/admittance_rule.cpp src/incremental_ik_calculator.cpp ) target_include_directories( admittance_controller PUBLIC + $ + $ ${EIGEN3_INCLUDE_DIR} - include ) ament_target_dependencies( admittance_controller + control_msgs controller_interface ${Eigen_LIBRARIES} + geometry_msgs hardware_interface + iirob_filters moveit_ros_move_group moveit_ros_planning_interface pluginlib rclcpp rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros ) # prevent pluginlib from using boost target_compile_definitions(admittance_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") @@ -65,14 +83,46 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_admittance_controller test/test_load_admittance_controller.cpp) - target_include_directories(test_admittance_controller PRIVATE include) + ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) + target_include_directories(test_load_admittance_controller PRIVATE include) ament_target_dependencies( - test_admittance_controller + test_load_admittance_controller controller_manager hardware_interface ros2_control_test_assets ) + + ament_add_gmock(test_admittance_controller test/test_admittance_controller.cpp) + target_include_directories(test_admittance_controller PRIVATE include) + target_link_libraries(test_admittance_controller admittance_controller) + ament_target_dependencies( + test_admittance_controller + control_msgs + controller_interface + hardware_interface + ) endif() +ament_export_include_directories( + include +) +ament_export_libraries( + admittance_controller +) +ament_export_dependencies( + control_msgs + controller_interface + geometry_msgs + hardware_interface + iirob_filters + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros +) + ament_package() diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index b8032213f9..fc815e14e2 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -17,11 +17,25 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ -#include "admittance_controller/incremental_ik_calculator.hpp" +#include +#include +#include + +#include "admittance_controller/admittance_rule.hpp" #include "admittance_controller/visibility_control.h" +#include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "iirob_filters/low_pass_filter.h" +#include "iirob_filters/gravity_compensation.h" +#include "semantic_components/force_torque_sensor.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" namespace admittance_controller { @@ -56,14 +70,59 @@ class AdmittanceController : public controller_interface::ControllerInterface protected: std::vector joint_names_; - std::vector interface_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + std::string ft_sensor_name_; + + // Internal variables + std::unique_ptr force_torque_sensor_; + + // Admittance rule and dependent variables; + std::unique_ptr admittance_; + rclcpp::Time previous_time_; + + // Command subscribers and Controller State publisher + using ControllerCommandForceMsg = geometry_msgs::msg::WrenchStamped; + using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; + + rclcpp::Subscription::SharedPtr + input_force_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + input_pose_command_subscriber_ = nullptr; + + realtime_tools::RealtimeBuffer> + input_force_command_; + realtime_tools::RealtimeBuffer> + input_pose_command_; + + using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // Internal access to sorted interfaces + + // To reduce number of variables and to make the code shorter the interfaces are ordered in types + // as the following constants + const std::vector allowed_interface_types_ = { + hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, + }; + + // The interfaces are defined as the types in 'allowed_interface_types_' member. + // For convenience, for each type the interfaces are ordered so that i-th position + // matches i-th index in joint_names_ + template + using InterfaceReferences = std::vector>>; + + InterfaceReferences joint_command_interface_; + InterfaceReferences joint_state_interface_; - std::shared_ptr ik_; + bool has_velocity_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; - Eigen::VectorXd delta_x_; - Eigen::VectorXd delta_theta_; - // TODO(andyz): parameterize this - std::string force_torque_sensor_frame_; }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp new file mode 100644 index 0000000000..f21bf98c8f --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -0,0 +1,162 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ + +#include "admittance_controller/incremental_ik_calculator.hpp" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +namespace admittance_controller +{ +class AdmittanceRule +{ +public: + AdmittanceRule() = default; + + controller_interface::return_type configure(rclcpp::Clock::SharedPtr clock); + + controller_interface::return_type update( + const std::vector & current_joint_state, + const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::PoseStamped & target_pose, + const geometry_msgs::msg::WrenchStamped & target_force, + const rclcpp::Duration & period + ); + + controller_interface::return_type update( + const std::vector & current_joint_state, + const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::PoseStamped & target_pose, + const rclcpp::Duration & period); + +// controller_interface::return_type update( +// const geometry_msgs::msg::WrenchStamped & measured_force, +// const geometry_msgs::msg::PoseStamped & target_pose, +// const geometry_msgs::msg::PoseStamped & current_pose, +// const rclcpp::Duration & period, +// geometry_msgs::msg::TransformStamped & relative_desired_pose_vec +// ); + + controller_interface::return_type get_controller_state( + control_msgs::msg::AdmittanceControllerState & state_message + ); + + controller_interface::return_type reset(); + +public: + // IK related parameters + std::string ik_base_frame_; + std::string ik_tip_frame_; + std::string ik_group_name_; + + // Controller frames + std::string control_frame_; + std::string endeffector_frame_; + std::string fixed_world_frame_; + std::string sensor_frame_; + + // Admittance parameters + // bool unified_mode_ = false; + std::array selected_axes_; + std::array mass_; + std::array damping_; + std::array stiffness_; + +protected: + // IK variables + std::shared_ptr ik_; + Eigen::VectorXd delta_x_; + Eigen::VectorXd delta_theta_; + + // Transformation variables + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + geometry_msgs::msg::WrenchStamped measured_force_; + geometry_msgs::msg::WrenchStamped measured_force_control_frame_; + + geometry_msgs::msg::PoseStamped current_origin_; + geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::PoseStamped current_pose_control_frame_; + + geometry_msgs::msg::WrenchStamped target_force_control_frame_; + geometry_msgs::msg::PoseStamped target_pose_control_frame_; + + geometry_msgs::msg::PoseStamped desired_pose_; + geometry_msgs::msg::TransformStamped relative_desired_pose_; + + // Pre-reserved update-loop variables + std::array measured_force_control_frame_vec_; + std::array target_pose_control_frame_vec_; + std::array current_pose_control_frame_vec_; + + std::array angles_error_; + + std::array relative_desired_pose_vec_; + std::array desired_velocity_vec_; + std::array desired_velocity_previous_vec_; + std::array desired_acceleration_previous_vec_; + +private: + // TODO: implement doTransform for WrenchStamped + template +// using MsgType = geometry_msgs::msg::PoseStamped; + void transform_message_to_control_frame(const MsgType & message_in, MsgType & message_out) + { + if (control_frame_ != message_in.header.frame_id) { + geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( + control_frame_, message_in.header.frame_id, tf2::TimePointZero); + tf2::doTransform(message_in, message_out, transform); + } else { + message_out = message_in; + } + } + +// using MsgType2 = geometry_msgs::msg::WrenchStamped; +// void transform_message_to_control_frame(const MsgType2 & message_in, MsgType2 & message_out) +// { +// if (control_frame_ != message_in.header.frame_id) { +// geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( +// control_frame_, message_in.header.frame_id, tf2::TimePointZero); +// +// geometry_msgs::msg::Vector3Stamped vec_in; +// geometry_msgs::msg::Vector3Stamped vec_out; +// +// vec_in.vector = message_in.wrench.force; +// vec_in.header = message_in.header; +// tf2::doTransform(vec_in, vec_out, transform); +// message_out.wrench.force = vec_out.vector; +// +// vec_in.vector = message_in.wrench.torque; +// tf2::doTransform(vec_in, vec_out, transform); +// message_out.wrench.torque = vec_out.vector; +// } else { +// message_out = message_in; +// } +// } +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index 8533e45eaf..2bfd17a4b8 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include namespace admittance_controller @@ -32,7 +31,7 @@ class IncrementalIKCalculator * \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta. * It uses the Jacobian from MoveIt. */ - IncrementalIKCalculator(std::shared_ptr& node); + IncrementalIKCalculator(std::shared_ptr & node); /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 43c58b078f..456296e972 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -9,19 +9,26 @@ ament_cmake + control_msgs controller_interface + geometry_msgs hardware_interface + iirob_filters moveit_ros_move_group moveit_ros_planning_interface pluginlib rclcpp rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros ament_cmake_gmock + control_msgs controller_manager hardware_interface - moveit_ros_move_group - moveit_ros_planning_interface ros2_control_test_assets diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c71d979bfe..78d270cb36 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -14,11 +14,16 @@ // /// \author: Denis Stogl +#include +#include #include #include +// #include "Eigen/Core" + #include "admittance_controller/admittance_controller.hpp" -#include "Eigen/Core" +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" namespace admittance_controller { @@ -35,40 +40,249 @@ controller_interface::return_type AdmittanceController::init(const std::string & } try { - auto node = get_node(); - node->declare_parameter>("joints", {}); - node->declare_parameter("interface_name", ""); - ik_ = std::make_shared(node); - delta_x_ = Eigen::VectorXd(6); - force_torque_sensor_frame_ = "force_sensor"; + get_node()->declare_parameter>("joints", {}); + get_node()->declare_parameter>("command_interfaces", {}); + get_node()->declare_parameter>("state_interfaces", {}); + get_node()->declare_parameter("ft_sensor_name", ""); + + get_node()->declare_parameter("IK.base", ""); + get_node()->declare_parameter("IK.tip", ""); + // TODO(destogl): enable when IK support is added +// get_node()->declare_parameter("IK.plugin", ""); +// get_node()->declare_parameter("IK.group_name", ""); + + get_node()->declare_parameter("control_frame", ""); + get_node()->declare_parameter("endeffector_frame", ""); + get_node()->declare_parameter("fixed_world_frame", ""); + get_node()->declare_parameter("sensor_frame", ""); + + get_node()->declare_parameter>("gravity_compensation.masses", {}); + get_node()->declare_parameter>("gravity_compensation.center_of_masses", {}); + + // TODO(destogl): enable when force/position control is implemented +// get_node()->declare_parameter("admitance.unified_mode", false); + get_node()->declare_parameter("admittance.selected_axes.x", false); + get_node()->declare_parameter("admittance.selected_axes.y", false); + get_node()->declare_parameter("admittance.selected_axes.z", false); + get_node()->declare_parameter("admittance.selected_axes.rx", false); + get_node()->declare_parameter("admittance.selected_axes.ry", false); + get_node()->declare_parameter("admittance.selected_axes.rz", false); + + get_node()->declare_parameter("admittance.mass.x", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.y", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.z", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.rx", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.ry", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.rz", std::numeric_limits::quiet_NaN()); + + get_node()->declare_parameter("admittance.damping.x", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.y", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.z", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.rx", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.ry", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.rz", std::numeric_limits::quiet_NaN()); + + get_node()->declare_parameter("admittance.stiffness.x", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.y", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.z", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.rx", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.ry", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.rz", std::numeric_limits::quiet_NaN()); + +// get_node()->declare_parameter>("") } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } + admittance_ = std::make_unique(); + + auto node = get_node(); +// ik_ = std::make_shared(node); +// delta_x_ = Eigen::VectorXd(6); + return controller_interface::return_type::OK; } CallbackReturn AdmittanceController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - joint_names_ = node_->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + auto error_if_empty = [&](const auto & parameter, const char * parameter_name) { + if (parameter.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was empty", parameter_name); + return true; + } + return false; + }; + + auto get_string_array_param_and_error_if_empty = [&]( + std::vector & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).as_string_array(); + return error_if_empty(parameter, parameter_name); + }; + + auto get_string_param_and_error_if_empty = [&]( + std::string & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).as_string(); + return error_if_empty(parameter, parameter_name); + }; + + // TODO(destogl): If we would use C++20 than we can use templates here + auto get_bool_param_and_error_if_empty = [&]( + bool & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).get_value(); + return false; // TODO(destogl): how to check "if_empty" for bool? + }; + + auto get_double_param_and_error_if_empty = [&]( + double & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).get_value(); + if (std::isnan(parameter)) { + RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was not set", parameter_name); + return true; + } + return false; + }; + + if ( + get_string_array_param_and_error_if_empty(joint_names_, "joints") || + get_string_array_param_and_error_if_empty(command_interface_types_, "command_interfaces") || + get_string_array_param_and_error_if_empty(state_interface_types_, "state_interfaces") || + get_string_param_and_error_if_empty(ft_sensor_name_, "ft_sensor_name") || + get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") || + get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || + // TODO: Enable this when IK plugin use is implemented + // get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || + get_string_param_and_error_if_empty(admittance_->control_frame_, "control_frame") || + get_string_param_and_error_if_empty(admittance_->endeffector_frame_, "endeffector_frame") || + get_string_param_and_error_if_empty(admittance_->fixed_world_frame_, "fixed_world_frame") || + get_string_param_and_error_if_empty(admittance_->sensor_frame_, "sensor_frame") || + // TODO(destogl): add unified mode considering target force +// get_bool_param_and_error_if_empty(unified_mode_, "admittance.unified_mode") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[0], "admittance.selected_axes.x") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[1], "admittance.selected_axes.y") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[2], "admittance.selected_axes.z") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[3], "admittance.selected_axes.rx") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[4], "admittance.selected_axes.ry") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[5], "admittance.selected_axes.rz") || + + get_double_param_and_error_if_empty(admittance_->mass_[0], "admittance.mass.x") || + get_double_param_and_error_if_empty(admittance_->mass_[1], "admittance.mass.y") || + get_double_param_and_error_if_empty(admittance_->mass_[2], "admittance.mass.z") || + get_double_param_and_error_if_empty(admittance_->mass_[3], "admittance.mass.rx") || + get_double_param_and_error_if_empty(admittance_->mass_[4], "admittance.mass.ry") || + get_double_param_and_error_if_empty(admittance_->mass_[5], "admittance.mass.rz") || + + get_double_param_and_error_if_empty(admittance_->damping_[0], "admittance.damping.x") || + get_double_param_and_error_if_empty(admittance_->damping_[1], "admittance.damping.y") || + get_double_param_and_error_if_empty(admittance_->damping_[2], "admittance.damping.z") || + get_double_param_and_error_if_empty(admittance_->damping_[3], "admittance.damping.rx") || + get_double_param_and_error_if_empty(admittance_->damping_[4], "admittance.damping.ry") || + get_double_param_and_error_if_empty(admittance_->damping_[5], "admittance.damping.rz") || + + get_double_param_and_error_if_empty(admittance_->stiffness_[0], "admittance.stiffness.x") || + get_double_param_and_error_if_empty(admittance_->stiffness_[1], "admittance.stiffness.y") || + get_double_param_and_error_if_empty(admittance_->stiffness_[2], "admittance.stiffness.z") || + get_double_param_and_error_if_empty(admittance_->stiffness_[3], "admittance.stiffness.rx") || + get_double_param_and_error_if_empty(admittance_->stiffness_[4], "admittance.stiffness.ry") || + get_double_param_and_error_if_empty(admittance_->stiffness_[5], "admittance.stiffness.rz") + ) + { return CallbackReturn::ERROR; } - // Specialized, child controllers set interfaces before calling init function. - if (interface_names_.empty()) { - interface_names_ = node_->get_parameter("interface_names").as_string_array(); + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + joint_command_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : command_interface_types_) { + auto it = std::find( + allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) { + RCLCPP_ERROR(get_node()->get_logger(), "Command interface type '" + interface + "' not allowed!"); + return CallbackReturn::ERROR; + } + } + + if (controller_interface::interface_list_contains_interface_type( + command_interface_types_, hardware_interface::HW_IF_POSITION)) { + has_position_command_interface_ = true; + } + if (controller_interface::interface_list_contains_interface_type( + command_interface_types_, hardware_interface::HW_IF_VELOCITY)) { + has_velocity_command_interface_ = true; } - if (interface_names_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + joint_state_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : state_interface_types_) { + auto it = std::find( + allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) { + RCLCPP_ERROR(get_node()->get_logger(), "State interface type '" + interface + "' not allowed!"); + return CallbackReturn::ERROR; + } + } + + if (!controller_interface::interface_list_contains_interface_type( + state_interface_types_, hardware_interface::HW_IF_POSITION)) { + RCLCPP_ERROR(get_node()->get_logger(), "State interface type '" + std::string(hardware_interface::HW_IF_POSITION) + "' has to be always present allowed!"); return CallbackReturn::ERROR; } + if (controller_interface::interface_list_contains_interface_type( + state_interface_types_, hardware_interface::HW_IF_VELOCITY)) { + has_velocity_state_interface_ = true; + } + + auto get_interface_list = [](const std::vector & interface_types) { + std::stringstream ss_command_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) { + if (index != 0) { + ss_command_interfaces << " "; + } + ss_command_interfaces << interface_types[index]; + } + return ss_command_interfaces.str(); + }; + + // Print output so users can be sure the interface setup is correct + RCLCPP_INFO( + get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", + get_interface_list(command_interface_types_).c_str(), + get_interface_list(state_interface_types_).c_str()); + + // Initialize FTS semantic semantic_component + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(ft_sensor_name_)); + + // Subscribers and callbacks + auto callback_input_force = [&](const std::shared_ptr msg) + -> void + { + input_force_command_.writeFromNonRT(msg); + }; + input_force_command_subscriber_ = get_node()->create_subscription( + "~/force_commands", rclcpp::SystemDefaultsQoS(), callback_input_force); + + auto callback_input_pose = [&](const std::shared_ptr msg) + -> void + { + input_pose_command_.writeFromNonRT(msg); + }; + input_pose_command_subscriber_ = get_node()->create_subscription( + "~/pose_commands", rclcpp::SystemDefaultsQoS(), callback_input_pose); + + // TODO(destogl): Add subscriber for velocity scaling + + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + + // Configure AdmittanceRule + admittance_->configure(get_node()->get_clock()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -79,8 +293,9 @@ const controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.reserve(joint_names_.size() * command_interface_types_.size()); for (const auto & joint : joint_names_) { - for (const auto & interface : interface_names_) { + for (const auto & interface : command_interface_types_) { command_interfaces_config.names.push_back(joint + "/" + interface); } } @@ -94,8 +309,12 @@ const controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names.reserve(6 + joint_names_.size() * state_interface_types_.size()); + + state_interfaces_config.names = force_torque_sensor_->get_state_interface_names(); + for (const auto & joint : joint_names_) { - for (const auto & interface : interface_names_) { + for (const auto & interface : state_interface_types_) { state_interfaces_config.names.push_back(joint + "/" + interface); } } @@ -103,74 +322,106 @@ const return state_interfaces_config; } -// Fill ordered_interfaces with references to the matching interfaces -// in the same order as in joint_names -template -bool get_ordered_interfaces( - std::vector & unordered_interfaces, - const std::vector & joint_names, - const std::string & interface_type, - std::vector> & ordered_interfaces) +CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - for (const auto & joint_name : joint_names) { - for (auto & command_interface : unordered_interfaces) { - if ((command_interface.get_name() == joint_name) && - (command_interface.get_interface_name() == interface_type)) - { - ordered_interfaces.push_back(std::ref(command_interface)); - } + // order all joints in the storage + for (const auto & interface : command_interface_types_) { + auto it = std::find( + allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + node_->get_logger(), "Expected %u '%s' command interfaces, got %u.", + joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + for (const auto & interface : state_interface_types_) { + auto it = std::find( + allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + { + RCLCPP_ERROR( + node_->get_logger(), "Expected %u '%s' state interfaces, got %u.", + joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } - return joint_names.size() == ordered_interfaces.size(); -} + // Initialize interface of the FTS semantic semantic component + force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); + + // Initialize Admittance Rule from current states + admittance_->reset(); + previous_time_ = get_node()->now(); + + // Set initial command values + std::shared_ptr msg_force = std::make_shared(); + msg_force->header.frame_id = admittance_->control_frame_; + input_force_command_.writeFromNonRT(msg_force); + + std::shared_ptr msg_pose = std::make_shared(); + msg_pose->header.frame_id = admittance_->control_frame_; + input_pose_command_.writeFromNonRT(msg_pose); -CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -{ return CallbackReturn::SUCCESS; } CallbackReturn AdmittanceController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { + // Stop the robot + for (auto index = 0ul; index < joint_names_.size(); ++index) { + joint_command_interface_[0][index].get().set_value( + joint_command_interface_[0][index].get().get_value()); + } + + for (auto index = 0ul; index < allowed_interface_types_.size(); ++index) { + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } + release_interfaces(); + + force_torque_sensor_->release_interfaces(); + return CallbackReturn::SUCCESS; } controller_interface::return_type AdmittanceController::update() { - // Task space admittance pipeline: - // Get wrench measurement - // Calculate the desired Cartesian displacement of the robot with the admittance equation. - // The basic form is: F = K * (x_d - x) - // (wrench) = (stiffness matrix) * (desired_Cartesian_position minus current_Cartesian_position) - // Damping terms can be added too. - // Get current robot joint angles - // Convert Cartesian deltas to joint angle deltas via Jacobian - // Write new joint angles to robot - - - // Get wrench measurement - - // Calculate desired Cartesian displacement of the robot - //delta_x_ = ... + // get input commands + // TODO(destogl): Enable this when unified mode is used + auto input_force_cmd = input_force_command_.readFromRT(); + auto input_pose_cmd = input_pose_command_.readFromRT(); - // Get current robot joint angles + // Position has to always there + auto num_joints = joint_state_interface_[0].size(); + std::vector current_joint_states(num_joints); - // Convert Cartesian deltas to joint angle deltas via Jacobian - if (!ik_->convertCartesianDeltasToJointDeltas(delta_x_, force_torque_sensor_frame_, delta_theta_)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); - return controller_interface::return_type::ERROR; + for (auto i = 0u; i < num_joints; ++i) { + current_joint_states.emplace_back(joint_state_interface_[0][i].get().get_value()); } - // Write new joint angles to the robot - + // TODO(destogl): Enable this when unified mode is used +// if (admittance_.unified_mode_) { +// admittance_->update(current_joint_states, force_torque_sensor_->get_values_as_message(), **input_pose_cmd, **input_force_cmd, get_node()->now() - previous_time_); +// } else { + admittance_->update(current_joint_states, force_torque_sensor_->get_values_as_message(), **input_pose_cmd, get_node()->now() - previous_time_); +// } + previous_time_ = get_node()->now(); + // Write new joint angles to the robot - // TODO: below is just a skeleton - for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value(state_interfaces_[index].get_value()); - } + // Publish controller state + state_publisher_->lock(); + state_publisher_->msg_.input_force_command = **input_force_cmd; + state_publisher_->msg_.input_pose_command = **input_pose_cmd; + admittance_->get_controller_state(state_publisher_->msg_); + state_publisher_->unlockAndPublish(); return controller_interface::return_type::OK; } diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp new file mode 100644 index 0000000000..54220f589f --- /dev/null +++ b/admittance_controller/src/admittance_rule.cpp @@ -0,0 +1,286 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#include "admittance_controller/admittance_rule.hpp" + +#include "angles/angles.h" + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/utilities.hpp" +#include "tf2/utils.h" + +namespace { // Utility namespace + +void get_rpy_difference_between_two_quaternions(const geometry_msgs::msg::Quaternion quat1, + const geometry_msgs::msg::Quaternion quat2, std::array vector_out) +{ + tf2::Quaternion q1(quat1.x, quat1.y, quat1.z, quat1.w); + tf2::Quaternion q2(quat2.x, quat2.y, quat2.z, quat2.w); + + // q1 - q2 + const tf2::Quaternion q_diff = q1 * q2.inverse(); + tf2::Matrix3x3(q_diff).getRPY(vector_out[0], vector_out[1], vector_out[2]); +} + +void convert_message_to_array(const geometry_msgs::msg::Pose & msg, std::array & vector_out) +{ + vector_out[0] = msg.position.x; + vector_out[1] = msg.position.y; + vector_out[2] = msg.position.z; + tf2::Quaternion q; + tf2::fromMsg(msg.orientation, q); + q.normalize(); + tf2::Matrix3x3(q).getRPY(vector_out[3], vector_out[4], vector_out[5]); + for (auto i = 3u; i < 6; ++i) { + vector_out[i] = angles::normalize_angle(vector_out[i]); + } +} + +void convert_message_to_array( + const geometry_msgs::msg::PoseStamped & msg, std::array & vector_out) +{ + convert_message_to_array(msg.pose, vector_out); +} + +void convert_message_to_array( + const geometry_msgs::msg::Wrench & msg, std::array & vector_out) +{ + vector_out[0] = msg.force.x; + vector_out[1] = msg.force.y; + vector_out[2] = msg.force.z; + vector_out[3] = msg.torque.x; + vector_out[4] = msg.torque.y; + vector_out[5] = msg.torque.z; +} + +void convert_message_to_array( + const geometry_msgs::msg::WrenchStamped & msg, std::array & vector_out) +{ + convert_message_to_array(msg.wrench, vector_out); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Pose & msg_out) +{ + msg_out.position.x = vector[0]; + msg_out.position.y = vector[1]; + msg_out.position.z = vector[2]; + + tf2::Quaternion q; + q.setRPY(vector[3], vector[4], vector[5]); + + msg_out.orientation.x = q.x(); + msg_out.orientation.y = q.y(); + msg_out.orientation.z = q.z(); + msg_out.orientation.w = q.w(); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::PoseStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.pose); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Wrench & msg_out) +{ + msg_out.force.x = vector[0]; + msg_out.force.y = vector[1]; + msg_out.force.z = vector[2]; + msg_out.torque.x = vector[3]; + msg_out.torque.y = vector[4]; + msg_out.torque.z = vector[5]; + +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::WrenchStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.wrench); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Transform & msg_out) +{ + msg_out.translation.x = vector[0]; + msg_out.translation.y = vector[1]; + msg_out.translation.z = vector[2]; + + tf2::Quaternion q; + q.setRPY(vector[3], vector[4], vector[5]); + + msg_out.rotation.x = q.x(); + msg_out.rotation.y = q.y(); + msg_out.rotation.z = q.z(); + msg_out.rotation.w = q.w(); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::TransformStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.transform); +} + +} // utility namespace + +namespace admittance_controller +{ + +controller_interface::return_type AdmittanceRule::configure(rclcpp::Clock::SharedPtr clock) +{ + tf_buffer_ = std::make_shared(clock); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Initialize variables used in the update loop + measured_force_.header.frame_id = sensor_frame_; + + current_origin_.header.frame_id = endeffector_frame_; + current_origin_.pose.orientation.w = 1; + current_pose_.header.frame_id = endeffector_frame_; + + relative_desired_pose_.header.frame_id = control_frame_; + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::reset() +{ + measured_force_control_frame_vec_.fill(0.0); + target_pose_control_frame_vec_.fill(0.0); + + current_pose_control_frame_vec_.fill(0.0); + + angles_error_.fill(0.0); + + desired_velocity_vec_.fill(0.0); + desired_velocity_previous_vec_.fill(0.0); + desired_acceleration_previous_vec_.fill(0.0); + + // - use tf2 instead of FK + // - transform data to Controller frame + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::update( + const std::vector & /*current_joint_state*/, + const geometry_msgs::msg::Wrench & /*measured_force*/, + const geometry_msgs::msg::PoseStamped & /*target_pose*/, + const geometry_msgs::msg::WrenchStamped & /*target_force*/, + const rclcpp::Duration & /*period*/ +) +{ + // TODO(destogl): Implement this update + // transform_message_to_control_frame(**input_force_cmd, force_cmd_ctrl_frame_); + // TODO(destogl) reuse ting from other update + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::update( + const std::vector & /*current_joint_state*/, + const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::PoseStamped & target_pose, + const rclcpp::Duration & period) +{ + // Convert inputs to control frame + transform_message_to_control_frame(target_pose, target_pose_control_frame_); + + // get current states, and transform those into controller frame + measured_force_.wrench = measured_force; + // TODO(destogl): Add gravity compensation of measured forces + measured_force_control_frame_ = measured_force_; + transform_message_to_control_frame(measured_force_, measured_force_control_frame_); + + // Get tool frame position - in the future use: IKSolver->getPositionFK(...) or similar (ask TODO(AndyZe)) + auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); + tf2::doTransform(current_origin_, current_pose_, transform); + transform_message_to_control_frame(current_pose_, current_pose_control_frame_); + + // Convert all data to arrays for simpler calculation + convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_vec_); + convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_vec_); + convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_vec_); + + // Use angle difference calculated with quaternions to avoid confusion with angles + get_rpy_difference_between_two_quaternions(target_pose_control_frame_.pose.orientation, + current_pose_control_frame_.pose.orientation, angles_error_); + + // Compute admittance control law: F = M*a + D*v + K*(x_d - x) + for (auto i = 0u; i < 6; ++i) { + if (selected_axes_[i]) { + double pose_error = target_pose_control_frame_vec_[i] - measured_force_control_frame_vec_[i]; + if (i >= 3) { + pose_error = angles_error_[i-3]; + } + + // TODO(destogl): check if velocity is measured from hardware + const double acceleration = 1 / mass_[i] * + (measured_force_control_frame_vec_[i] - damping_[i] * desired_velocity_vec_[i] - + stiffness_[i] * pose_error); + + desired_velocity_vec_[i] += (desired_acceleration_previous_vec_[i] + acceleration) * 0.5 * period.seconds(); + + relative_desired_pose_vec_[i] = (desired_velocity_previous_vec_[i] + desired_velocity_vec_[i]) * 0.5 * period.seconds(); + + desired_acceleration_previous_vec_[i] = acceleration; + desired_velocity_previous_vec_[i] = desired_velocity_vec_[i]; + } + } + + // Do clean conversion to the goal pose using transform and not messing with Euler angles + convert_array_to_message(relative_desired_pose_vec_, relative_desired_pose_); + tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); + + // Use Jacobian-based IK + // Calculate desired Cartesian displacement of the robot + //delta_x_ = relative_desired_pose_; + + // Get current robot joint angles + + // Convert Cartesian deltas to joint angle deltas via Jacobian + // if (!ik_->convertCartesianDeltasToJointDeltas(delta_x_, sensor_frame_, delta_theta_)) + // { + // RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); + // return controller_interface::return_type::ERROR; + // } + + // TODO(anyone: enable this when enabling use of IK directly + // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); + // tf2::doTransform(desired_pose_, ik_input_pose_, transform); + // ik_input_pose_.pose = endeffector_to_ik_tip(ik_input_pose_); + // std::vector ik_sol = ik_solver_->getPositionIK ( ); ... + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::get_controller_state( + control_msgs::msg::AdmittanceControllerState & state_message) +{ + // state_message.input_force_control_frame = target_force_control_frame_; + state_message.input_pose_control_frame = target_pose_control_frame_; + state_message.measured_force = measured_force_; + state_message.measured_force_control_frame = measured_force_control_frame_; + state_message.measured_force_endeffector_frame = measured_force_control_frame_; + state_message.desired_pose = desired_pose_; + state_message.relative_desired_pose = relative_desired_pose_; + // state_message.desired_joint_states = desired_pose_; + // state_message.actual_joint_states = desired_pose_; + // state_message.error_joint_state = desired_pose_; + + return controller_interface::return_type::OK; +} + +} // namespace admittance_controller diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index ace07c39ee..78991774de 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -16,6 +16,8 @@ #include "admittance_controller/incremental_ik_calculator.hpp" +#include "tf2_eigen/tf2_eigen.h" + namespace admittance_controller { IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& node) : node_(node) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp new file mode 100644 index 0000000000..75fc8d8e28 --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -0,0 +1,368 @@ +// Copyright (c) 2021, salfkjsadf +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_admittance_controller.hpp" + +#include +#include +#include +#include + +// When there are many mandatory parameters, set all by default and remove one by one in a +// parameterized test +TEST_P(AdmittanceControllerTestParameterizedParameters, one_parameter_is_missing) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +// TODO(anyone): the new gtest version afer 1.8.0 uses INSTANTIATE_TEST_SUITE_P +INSTANTIATE_TEST_CASE_P( + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedParameters, + ::testing::Values( + std::make_tuple( + std::string("joints"), + rclcpp::ParameterValue(std::vector()) + ), + std::make_tuple( + std::string("command_interfaces"), + rclcpp::ParameterValue(std::vector()) + ), + std::make_tuple( + std::string("state_interfaces"), + rclcpp::ParameterValue(std::vector()) + ), + std::make_tuple( + std::string("ft_sensor_name"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("IK.base"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("IK.tip"), + rclcpp::ParameterValue("") + ), +// std::make_tuple( +// std::string("IK.group_name"), +// rclcpp::ParameterValue("") +// ), + std::make_tuple( + std::string("control_frame"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("endeffector_frame"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("fixed_world_frame"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("sensor_frame"), + rclcpp::ParameterValue("") + ), + // TODO(anyone): this tests are unstable... +// std::make_tuple( +// std::string("admittance.selected_axes.x"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.y"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.z"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.rx"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.ry"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.rz"), +// rclcpp::ParameterValue(false) +// ), + std::make_tuple( + std::string("admittance.mass.x"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.y"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.z"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.rx"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.ry"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.rz"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.x"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.y"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.z"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.rx"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.ry"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.rz"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.x"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.y"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.z"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.rx"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.ry"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.rz"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ) + ) +); + +TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->command_interface_types_.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(!controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->joint_names_.size() == joint_names_.size()); + ASSERT_TRUE(std::equal(controller_->joint_names_.begin(), controller_->joint_names_.end(), + joint_names_.begin(), joint_names_.end())); + + ASSERT_TRUE(!controller_->command_interface_types_.empty()); + ASSERT_TRUE(controller_->command_interface_types_.size() == command_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->command_interface_types_.begin(), controller_->command_interface_types_.end(), + command_interface_types_.begin(), command_interface_types_.end())); + + ASSERT_TRUE(!controller_->state_interface_types_.empty()); + ASSERT_TRUE(controller_->state_interface_types_.size() == state_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->state_interface_types_.begin(), controller_->state_interface_types_.end(), + state_interface_types_.begin(), state_interface_types_.end())); + + ASSERT_EQ(controller_->ft_sensor_name_, ft_sensor_name_); + ASSERT_EQ(controller_->admittance_->ik_base_frame_, ik_base_frame_); + ASSERT_EQ(controller_->admittance_->ik_tip_frame_, ik_tip_frame_); + // ASSERT_EQ(controller_->admittance_->ik_group_name_, ik_group_name_); + ASSERT_EQ(controller_->admittance_->endeffector_frame_, endeffector_frame_); + ASSERT_EQ(controller_->admittance_->fixed_world_frame_, fixed_world_frame_); + ASSERT_EQ(controller_->admittance_->sensor_frame_, sensor_frame_); + + ASSERT_TRUE(!controller_->admittance_->selected_axes_.empty()); + ASSERT_TRUE(controller_->admittance_->selected_axes_.size() == admittance_selected_axes_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->selected_axes_.begin(), controller_->admittance_->selected_axes_.end(), + admittance_selected_axes_.begin(), admittance_selected_axes_.end())); + + ASSERT_TRUE(!controller_->admittance_->mass_.empty()); + ASSERT_TRUE(controller_->admittance_->mass_.size() == admittance_mass_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->mass_.begin(), controller_->admittance_->mass_.end(), + admittance_mass_.begin(), admittance_mass_.end())); + + ASSERT_TRUE(!controller_->admittance_->damping_.empty()); + ASSERT_TRUE(controller_->admittance_->damping_.size() == admittance_damping_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->damping_.begin(), controller_->admittance_->damping_.end(), + admittance_damping_.begin(), admittance_damping_.end())); + + ASSERT_TRUE(!controller_->admittance_->stiffness_.empty()); + ASSERT_TRUE(controller_->admittance_->stiffness_.size() == admittance_stiffness_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->stiffness_.begin(), controller_->admittance_->stiffness_.end(), + admittance_stiffness_.begin(), admittance_stiffness_.end())); +} + +TEST_F(AdmittanceControllerTest, check_interfaces) +{ + SetUpController(); + + 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(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(controller_->state_interfaces_.size(), state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); +} + + +TEST_F(AdmittanceControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Check ordered interfaces + ASSERT_TRUE(!controller_->joint_command_interface_.empty()); + for (const auto & interface : command_interface_types_) { + auto it = std::find( + controller_->allowed_interface_types_.begin(), controller_->allowed_interface_types_.end(), interface); + auto index = std::distance(controller_->allowed_interface_types_.begin(), it); + + ASSERT_TRUE(controller_->joint_command_interface_[index].size() == joint_names_.size()); + } + + ASSERT_TRUE(!controller_->joint_state_interface_.empty()); + for (const auto & interface : state_interface_types_) { + auto it = std::find( + controller_->allowed_interface_types_.begin(), controller_->allowed_interface_types_.end(), interface); + auto index = std::distance(controller_->allowed_interface_types_.begin(), it); + + ASSERT_TRUE(controller_->joint_state_interface_[index].size() == joint_names_.size()); + } +} + +TEST_F(AdmittanceControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + assign_interfaces(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + broadcast_tfs(); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // Check default parameters + ASSERT_EQ(msg.input_force_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); +} + +TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + ASSERT_EQ(msg.input_force_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + broadcast_tfs(); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[0], 0.0); + + subscribe_and_get_messages(msg); + ASSERT_EQ(msg.input_force_command.header.frame_id, sensor_frame_); + ASSERT_EQ(msg.input_pose_command.header.frame_id, endeffector_frame_); +} + + +// Add test, wrong interfaces diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp new file mode 100644 index 0000000000..172f0b22aa --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -0,0 +1,389 @@ +// Copyright (c) 2021, salfkjsadf +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ADMITTANCE_CONTROLLER_HPP_ +#define TEST_ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "admittance_controller/admittance_controller.hpp" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "semantic_components/force_torque_sensor.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "tf2_ros/transform_broadcaster.h" + +// TODO(anyone): replace the state and command message types +using ControllerCommandForceMsg = geometry_msgs::msg::WrenchStamped; +using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +namespace +{ + constexpr auto NODE_SUCCESS = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + constexpr auto NODE_ERROR = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); + } + +} // namespace + +// subclassing and friending so we can access member varibles +class TestableAdmittanceController +: public admittance_controller::AdmittanceController +{ + FRIEND_TEST(AdmittanceControllerTest, joint_names_parameter_not_set); + FRIEND_TEST(AdmittanceControllerTest, interface_parameter_not_set); + FRIEND_TEST(AdmittanceControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AdmittanceControllerTest, check_interfaces); + FRIEND_TEST(AdmittanceControllerTest, activate_success); + +public: + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + admittance_controller::AdmittanceController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) { + input_force_command_subscriber_wait_set_.add_subscription(input_force_command_subscriber_); + input_pose_command_subscriber_wait_set_.add_subscription(input_pose_command_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds {500}) + { + bool success = + input_force_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready && + input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet input_force_command_subscriber_wait_set_; + rclcpp::WaitSet input_pose_command_subscriber_wait_set_; +}; + +class AdmittanceControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + force_command_publisher_ = command_publisher_node_->create_publisher( + "/test_admittance_controller/force_commands", rclcpp::SystemDefaultsQoS()); + pose_command_publisher_ = command_publisher_node_->create_publisher( + "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); + + test_subscription_node_ = std::make_shared("test_subscription_node"); + test_broadcaster_node_ = std::make_shared("test_broadcaster_node"); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void TearDown() + { + controller_.reset(nullptr); + } + +protected: + void SetUpController(bool set_parameters = true) + { + const auto result = controller_->init("test_admittance_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + assign_interfaces(); + + if (set_parameters) { + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"command_interfaces", command_interface_types_}); + controller_->get_node()->set_parameter({"state_interfaces", state_interface_types_}); + controller_->get_node()->set_parameter({"ft_sensor_name", ft_sensor_name_}); + + controller_->get_node()->set_parameter({"IK.base", ik_base_frame_}); + controller_->get_node()->set_parameter({"IK.tip", ik_tip_frame_}); + // TODO(destogl): enable when IK support is added +// controller_->get_node()->set_parameter({"IK.plugin", ik_group_name_}); +// controller_->get_node()->set_parameter({"IK.group_name", ik_group_name_}); + + controller_->get_node()->set_parameter({"control_frame", control_frame_}); + controller_->get_node()->set_parameter({"endeffector_frame", endeffector_frame_}); + controller_->get_node()->set_parameter({"fixed_world_frame", fixed_world_frame_}); + controller_->get_node()->set_parameter({"sensor_frame", sensor_frame_}); + + controller_->get_node()->set_parameter({"admittance.selected_axes.x", admittance_selected_axes_[0]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.y", admittance_selected_axes_[1]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.z", admittance_selected_axes_[2]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.rx", admittance_selected_axes_[3]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.ry", admittance_selected_axes_[4]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.rz", admittance_selected_axes_[5]}); + + controller_->get_node()->set_parameter({"admittance.mass.x", admittance_mass_[0]}); + controller_->get_node()->set_parameter({"admittance.mass.y", admittance_mass_[1]}); + controller_->get_node()->set_parameter({"admittance.mass.z", admittance_mass_[2]}); + controller_->get_node()->set_parameter({"admittance.mass.rx", admittance_mass_[3]}); + controller_->get_node()->set_parameter({"admittance.mass.ry", admittance_mass_[4]}); + controller_->get_node()->set_parameter({"admittance.mass.rz", admittance_mass_[5]}); + + controller_->get_node()->set_parameter({"admittance.damping.x", admittance_damping_[0]}); + controller_->get_node()->set_parameter({"admittance.damping.y", admittance_damping_[1]}); + controller_->get_node()->set_parameter({"admittance.damping.z", admittance_damping_[2]}); + controller_->get_node()->set_parameter({"admittance.damping.rx", admittance_damping_[3]}); + controller_->get_node()->set_parameter({"admittance.damping.ry", admittance_damping_[4]}); + controller_->get_node()->set_parameter({"admittance.damping.rz", admittance_damping_[5]}); + + controller_->get_node()->set_parameter({"admittance.stiffness.x", admittance_stiffness_[0]}); + controller_->get_node()->set_parameter({"admittance.stiffness.y", admittance_stiffness_[1]}); + controller_->get_node()->set_parameter({"admittance.stiffness.z", admittance_stiffness_[2]}); + controller_->get_node()->set_parameter({"admittance.stiffness.rx", admittance_stiffness_[3]}); + controller_->get_node()->set_parameter({"admittance.stiffness.ry", admittance_stiffness_[4]}); + controller_->get_node()->set_parameter({"admittance.stiffness.rz", admittance_stiffness_[5]}); + + } + } + + void assign_interfaces() + { + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (auto i = 0u; i < joint_command_values_.size(); ++i) { + command_itfs_.emplace_back( + hardware_interface::CommandInterface(joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + auto sc_fts = semantic_components::ForceTorqueSensor(ft_sensor_name_); + 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(); + state_itfs_.reserve(num_state_ifs); + state_ifs.reserve(num_state_ifs); + + for (auto i = 0u; i < joint_state_values_.size(); ++i) { + state_itfs_.emplace_back( + hardware_interface::StateInterface(joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector fts_itf_names = {"force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z"}; + + 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_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void broadcast_tfs() + { + static tf2_ros::TransformBroadcaster br(test_broadcaster_node_); + geometry_msgs::msg::TransformStamped transform_stamped; + + transform_stamped.header.stamp = test_broadcaster_node_.now(); + transform_stamped.header.frame_id = control_frame_; + transform_stamped.transform.translation.x = 0; + transform_stamped.transform.translation.y = 0; + transform_stamped.transform.translation.z = 1; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = sensor_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = endeffector_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = ik_base_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0; + transform_stamped.child_frame_id = fixed_world_frame_; + br.sendTransform(transform_stamped); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) + { + }; + auto subscription = + test_subscription_node_.create_subscription( + "/test_admittance_controller/state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + // wait for message to be passed + ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands() + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) { + if (wait_count >= 5) { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(force_command_publisher_->get_topic_name()); + wait_for_topic(pose_command_publisher_->get_topic_name()); + + ControllerCommandForceMsg force_msg; + force_msg.header.frame_id = sensor_frame_; + force_msg.wrench.force.x = 0.1; + force_msg.wrench.force.y = 0.2; + force_msg.wrench.force.z = 0.3; + force_msg.wrench.torque.x = 3.1; + force_msg.wrench.torque.y = 3.2; + force_msg.wrench.torque.z = 3.3; + + ControllerCommandPoseMsg pose_msg; + pose_msg.header.frame_id = endeffector_frame_; + pose_msg.pose.position.x = 0.1; + pose_msg.pose.position.y = 0.2; + pose_msg.pose.position.z = 0.3; + pose_msg.pose.orientation.x = 0; + pose_msg.pose.orientation.y = 0; + pose_msg.pose.orientation.z = 0; + pose_msg.pose.orientation.w = 1; + + force_command_publisher_->publish(force_msg); + pose_command_publisher_->publish(pose_msg); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + const std::vector joint_names_ = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; + const std::vector command_interface_types_ = {"position"}; + const std::vector state_interface_types_ = {"position"}; + const std::string ft_sensor_name_ = "ft_sensor_name"; + const std::string ik_base_frame_ = "IK_base"; + const std::string ik_tip_frame_ = "IK.tip"; + const std::string ik_group_name_ = "IK.group_name"; + const std::string control_frame_ = "control_frame"; + const std::string endeffector_frame_ = "endeffector_frame"; + const std::string fixed_world_frame_ = "fixed_world_frame"; + const std::string sensor_frame_ = "sensor_frame"; + + std::array admittance_selected_axes_ = {true, true, true, true, true, true}; + std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; + std::array admittance_damping_ = {100.1, 100.2, 100.3, 100.4, 100.5, 100.6}; + 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}; + std::vector fts_state_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr force_command_publisher_; + rclcpp::Publisher::SharedPtr pose_command_publisher_; + rclcpp::Node::SharedPtr test_subscription_node_; + rclcpp::Node::SharedPtr test_broadcaster_node_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class AdmittanceControllerTestParameterizedParameters +: public AdmittanceControllerTest, +public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() + { + AdmittanceControllerTest::SetUp(); + } + + static void TearDownTestCase() + { + AdmittanceControllerTest::TearDownTestCase(); + } + +protected: + void SetUpController(bool set_parameters = true) + { + AdmittanceControllerTest::SetUpController(set_parameters); + controller_->get_node()->undeclare_parameter(std::get<0>(GetParam())); + controller_->get_node()->declare_parameter(std::get<0>(GetParam()), std::get<1>(GetParam())); + } + +}; + +#endif // TEST_ADMITTANCE_CONTROLLER_HPP_ diff --git a/doc/admittance_controller.rst b/doc/admittance_controller.rst new file mode 100644 index 0000000000..f265c419b4 --- /dev/null +++ b/doc/admittance_controller.rst @@ -0,0 +1,58 @@ +admittance_controller +====================== + +The controller uses admittance dynamics to calculate robot movements caused by an external force. +The standard admittance control law: F = M\*a + D\*v + K\*(x_d - x) is used. + +The paramters are adjustable using ``dynamic_reconfigure`` functionality and rqt-plugin. + +The controller has following parameters: + + - ``joints`` - names of joints to control the robot. + - ``command_interfaces`` - interfaces uses for commanding the robot, e.g., position and/or velocity. + - ``state_interfaces`` - interfaces to get robot's current state. Currently only position is supported. + - ``ft_sensor_name`` - name of the sensor providing force-torque measurements. The controller uses `ForceTorqueSensor semantic component `_ which defines standard interface names for this measurements, e.g., /force.x. + + - ``IK.base`` - base link of the IK, usually the first fixed link on a robot, e.g., ``base_link``. + - ``IK.tip`` - end of the IK, usually flansh of the robot, e.g., ``tool0``. + - ``IK.group_name`` - name of the IK group defined when using a IK plugin (not implemented yet). + + - ``controller_frame`` - frame where all input and measured values will be transformed to do the calculation of admittance equation. + - ``endeffector_frame`` - frame of the endeffector, where force is influencing the robot and which pose is manipulated. + - ``fixed_world_frame`` - frame where gravity vector is oriented in ``-z`` direction. + - ``sensor_frame`` - frame where force-torque values are measured. + + - ``gravity_compensation.masses`` - list of masses of robot parts to subtract their gravity influence from force/torque measurements. + - ``gravity_compensation.center_of_masses`` - list of distances for the corresponding mass from ``sensor_frame`` (one dimensional). + + - ``admittance.selected_axes.[x|y|z|rx|ry|rz]`` - boolean value to enable admittance control for specific degree of freedom in ``controller_frame``. + - ``admittance.mass.[x|y|z|rx|ry|rz]`` - six virtual mass (M) values for degrees of freedom. + - ``admittance.damping.[x|y|z|rx|ry|rz]`` - six virtual damping (D) values for degrees of freedom. + - ``admittance.stiffness.[x|y|z|rx|ry|rz]`` - six virtual stiffness values (K) for degrees of freedom. + + +Publishers: + + - ``state`` - state message of the controller as ``control_msgs/msg/AdmittanceControllerState``: + + geometry_msgs/WrenchStamped input_force_command #commanded input force for the controller + geometry_msgs/PoseStamped input_pose_command #commanded input pose for the controller + + geometry_msgs/WrenchStamped input_force_transformed # input force transformed into control frame + geometry_msgs/PoseStamped input_pose_transformed # input pose transformed into control frame + + geometry_msgs/WrenchStamped measured_force # measured force from the sensor (sensor frame) + geometry_msgs/WrenchStamped measured_force_filtered # measured force after low-pass and gravity compensation filters in sensor frame + geometry_msgs/WrenchStamped measured_force_transformed # transformed measured force to control frame + + geometry_msgs/PoseStamped goal_pose_command # goal pose to be commanded + + string[] joint_names + trajectory_msgs/JointTrajectoryPoint desired_joint_states # result of IK from goal_pose_command + trajectory_msgs/JointTrajectoryPoint actual_joint_states # read from the hardware + trajectory_msgs/JointTrajectoryPoint error_joint_state + + +Subscribers: + + - ``input_pose_command`` - desired pose as ``geometry_msgs/msg/PoseStamped``