diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt new file mode 100644 index 0000000000..bbfe19deda --- /dev/null +++ b/admittance_controller/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.5) +project(admittance_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + kinematics_interface + Eigen3 + generate_parameter_library + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp) +target_include_directories(${PROJECT_NAME} PRIVATE include) +generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) +target_link_libraries(${PROJECT_NAME} admittance_controller_parameters) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(control_msgs REQUIRED) + find_package(controller_manager REQUIRED) + find_package(controller_interface REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ## create custom test function to pass yaml file into test main + #function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) + #add_executable(${TARGET} ${SOURCES}) + #_ament_cmake_gmock_find_gmock() + #target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") + #target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) + #set(executable "$") + #set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") + #ament_add_test( + #${TARGET} + #COMMAND ${executable} --ros-args --params-file ${YAML_FILE} + #--gtest_output=xml:${result_file} + #OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt + #RESULT_FILE ${result_file} + #) + #endfunction() + + # test loading admittance controller + add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) + target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) + ament_target_dependencies( + test_load_admittance_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + # test admittance controller function + add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + 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 + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_package() diff --git a/admittance_controller/admittance_controller.xml b/admittance_controller/admittance_controller.xml new file mode 100644 index 0000000000..3e4d5cb682 --- /dev/null +++ b/admittance_controller/admittance_controller.xml @@ -0,0 +1,9 @@ + + + + AdmittanceController ros2_control controller. + + + diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst new file mode 100644 index 0000000000..60837457f9 --- /dev/null +++ b/admittance_controller/doc/userdoc.rst @@ -0,0 +1,58 @@ +.. _joint_trajectory_controller_userdoc: + +Admittance Controller +====================== + +Admittance controller enables you do zero-force control from a force measured on your TCP. +The controller implements ``ChainedControllerInterface``, so it is possible to add another controllers in front of it, e.g., ``JointTrajectoryController``. + +The controller requires an external kinematics plugin to function. The `kinematics_interface `_ repository provides an interface and an implementation that the admittance controller can use. + +Parameters: + + + +ROS2 interface of the controller +--------------------------------- + +Parameters +^^^^^^^^^^^ + +The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. +The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +An example parameter file can be found in the `test folder of the controller `_ + + +Topics +^^^^^^^ + +~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] + Target joint commands when controller is not in chained mode. + +~/state (output topic) [control_msgs::msg::AdmittanceControllerState] + Topic publishing internal states. + + +ros2_control interfaces +------------------------ + +References +^^^^^^^^^^^ +The controller has ``position`` and ``velocity`` reference interfaces exported in the format: +``//[position|velocity]`` + + +States +^^^^^^^ +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +If some interface is not provided, the last commanded interface will be used for calculation. + +For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. +The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``. + + +Commands +^^^^^^^^^ +The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp new file mode 100644 index 0000000000..f776646d42 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -0,0 +1,187 @@ +// Copyright (c) 2022, 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. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include +#include + +// include generated parameter library +#include "admittance_controller_parameters.hpp" + +#include "admittance_controller/admittance_rule.hpp" +#include "admittance_controller/visibility_control.h" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "controller_interface/chainable_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 "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.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" +#include "semantic_components/force_torque_sensor.hpp" + +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +namespace admittance_controller +{ +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +class AdmittanceController : public controller_interface::ChainableControllerInterface +{ +public: + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + /// Export configuration of required state interfaces. + /** + * Allowed types of state interfaces are \ref hardware_interface::POSITION, + * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. + */ + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /// Export configuration of required state interfaces. + /** + * Allowed types of state interfaces are \ref hardware_interface::POSITION, + * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. + */ + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers() override; + + size_t num_joints_ = 0; + std::vector command_joint_names_; + + // 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_; + + bool has_position_state_interface_ = false; + bool has_velocity_state_interface_ = false; + bool has_acceleration_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; + bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; + + // 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, + 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_; + + // Admittance rule and dependent variables; + std::unique_ptr admittance_; + + // force torque sensor + std::unique_ptr force_torque_sensor_; + + // ROS subscribers + rclcpp::Subscription::SharedPtr + input_joint_command_subscriber_; + rclcpp::Publisher::SharedPtr s_publisher_; + + // admittance parameters + std::shared_ptr parameter_handler_; + + // ROS messages + std::shared_ptr joint_command_msg_; + + // real-time buffer + realtime_tools::RealtimeBuffer> + input_joint_command_; + std::unique_ptr> state_publisher_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; + trajectory_msgs::msg::JointTrajectoryPoint last_reference_; + + // control loop data + // reference_: reference value read by the controller + // joint_state_: current joint readings from the hardware + // reference_admittance_: reference value used by the controller after the admittance values are + // applied ft_values_: values read from the force torque sensor + trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; + geometry_msgs::msg::Wrench ft_values_; + + /** + * @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values + */ + void read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state_current, + geometry_msgs::msg::Wrench & ft_values); + + /** + * @brief Set fields of state_reference with values from controllers exported position and velocity references + */ + void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); + + /** +* @brief Write values from state_command to claimed hardware interfaces +*/ + void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command); +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ 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..0f0aabb063 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -0,0 +1,216 @@ +// 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. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "control_toolbox/filters.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "kinematics_interface/kinematics_interface.hpp" +#include "pluginlib/class_loader.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_kdl/tf2_kdl.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace admittance_controller +{ +struct AdmittanceTransforms +{ + // transformation from force torque sensor frame to base link frame at reference joint angles + Eigen::Isometry3d ref_base_ft_; + // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_ft_; + // transformation from control frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_control_; + // transformation from end effector frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_tip_; + // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_cog_; + // transformation from world frame to base link frame + Eigen::Isometry3d world_base_; +}; + +struct AdmittanceState +{ + explicit AdmittanceState(size_t num_joints) + { + admittance_velocity.setZero(); + admittance_acceleration.setZero(); + damping.setZero(); + mass.setOnes(); + mass_inv.setZero(); + stiffness.setZero(); + selected_axes.setZero(); + current_joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_vel = Eigen::VectorXd::Zero(num_joints); + joint_acc = Eigen::VectorXd::Zero(num_joints); + } + + Eigen::VectorXd current_joint_pos; + Eigen::VectorXd joint_pos; + Eigen::VectorXd joint_vel; + Eigen::VectorXd joint_acc; + Eigen::Matrix damping; + Eigen::Matrix mass; + Eigen::Matrix mass_inv; + Eigen::Matrix selected_axes; + Eigen::Matrix stiffness; + Eigen::Matrix wrench_base; + Eigen::Matrix admittance_acceleration; + Eigen::Matrix admittance_velocity; + Eigen::Isometry3d admittance_position; + Eigen::Matrix rot_base_control; + Eigen::Isometry3d ref_trans_base_ft; + std::string ft_sensor_frame; +}; + +class AdmittanceRule +{ +public: + explicit AdmittanceRule( + const std::shared_ptr & parameter_handler) + { + parameter_handler_ = parameter_handler; + parameters_ = parameter_handler_->get_params(); + num_joints_ = parameters_.joints.size(); + admittance_state_ = AdmittanceState(num_joints_); + reset(num_joints_); + } + + /// Configure admittance rule memory using number of joints. + controller_interface::return_type configure( + const std::shared_ptr & node, const size_t num_joint); + + /// Reset all values back to default + controller_interface::return_type reset(const size_t num_joints); + + /** + * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does + * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation + * are calculated without an error + * \param[in] current_joint_state current joint state of the robot + * \param[in] reference_joint_state input joint state reference + * \param[out] success true if no calls to the kinematics interface fail + */ + bool get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); + + /** + * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field + * members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + */ + void apply_parameters_update(); + + /** + * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and + * 'current_joint_state'. + * + * \param[in] current_joint_state current joint state of the robot + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] reference_joint_state input joint state reference + * \param[in] period time in seconds since last controller update + * \param[out] desired_joint_state joint state reference after the admittance offset is applied to the input reference + */ + controller_interface::return_type update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); + + /** + * Set fields of `state_message` from current admittance controller state. + * + * \param[out] state_message message containing target position/vel/accel, wrench, and actual robot state, among other things + */ + const control_msgs::msg::AdmittanceControllerState & get_controller_state(); + +public: + // admittance config parameters + std::shared_ptr parameter_handler_; + admittance_controller::Params parameters_; + +protected: + /** + * Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input + * is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin + * calls failed. + * \param[in] admittance_state contains all the information needed to calculate the admittance offset + * \param[in] dt controller period + * \param[out] success true if no calls to the kinematics interface fail + */ + bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); + + /** + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, + * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. + * The `wrench_world_` estimate includes gravity compensation + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame + * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame + */ + void process_wrench_measurements( + const geometry_msgs::msg::Wrench & measured_wrench, + const Eigen::Matrix & sensor_world_rot, + const Eigen::Matrix & cog_world_rot); + + template + void vec_to_eigen(const std::vector & data, T2 & matrix); + + // number of robot joint + size_t num_joints_; + + // Kinematics interface plugin loader + std::shared_ptr> + kinematics_loader_; + std::unique_ptr kinematics_; + + // filtered wrench in world frame + Eigen::Matrix wrench_world_; + + // admittance controllers internal state + AdmittanceState admittance_state_{0}; + + // transforms needed for admittance update + AdmittanceTransforms admittance_transforms_; + + // position of center of gravity in cog_frame + Eigen::Vector3d cog_pos_; + + // force applied to sensor due to weight of end effector + Eigen::Vector3d end_effector_weight_; + + // ROS + control_msgs::msg::AdmittanceControllerState state_message_; +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp new file mode 100644 index 0000000000..87c16e4787 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -0,0 +1,405 @@ +// Copyright (c) 2022, 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. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ + +#include "admittance_controller/admittance_rule.hpp" + +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/utilities.hpp" +#include "tf2_ros/transform_listener.h" + +namespace admittance_controller +{ +/// Configure admittance rule memory for num joints and load kinematics interface +controller_interface::return_type AdmittanceRule::configure( + const std::shared_ptr & node, const size_t num_joints) +{ + num_joints_ = num_joints; + + // initialize memory and values to zero (non-realtime function) + reset(num_joints); + + // Load the differential IK plugin + if (!parameters_.kinematics.plugin_name.empty()) + { + try + { + kinematics_loader_ = + std::make_shared>( + parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); + kinematics_ = std::unique_ptr( + kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize( + node->get_node_parameters_interface(), parameters_.kinematics.tip)) + { + return controller_interface::return_type::ERROR; + } + } + catch (pluginlib::PluginlibException & ex) + { + RCLCPP_ERROR( + rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", + parameters_.kinematics.plugin_name.c_str(), ex.what()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger("AdmittanceRule"), + "A differential IK plugin name was not specified in the config file."); + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) +{ + // reset state message fields + state_message_.joint_state.name.assign(num_joints, ""); + state_message_.joint_state.position.assign(num_joints, 0); + state_message_.joint_state.velocity.assign(num_joints, 0); + state_message_.joint_state.effort.assign(num_joints, 0); + for (size_t i = 0; i < parameters_.joints.size(); ++i) + { + state_message_.joint_state.name = parameters_.joints; + } + state_message_.mass.data.resize(6, 0.0); + state_message_.selected_axes.data.resize(6, 0); + state_message_.damping.data.resize(6, 0); + state_message_.stiffness.data.resize(6, 0); + state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.child_frame_id = "admittance_offset"; + + // reset admittance state + admittance_state_ = AdmittanceState(num_joints); + + // reset transforms and rotations + admittance_transforms_ = AdmittanceTransforms(); + + // reset forces + wrench_world_.setZero(); + end_effector_weight_.setZero(); + + // load/initialize Eigen types from parameters + apply_parameters_update(); + + return controller_interface::return_type::OK; +} + +void AdmittanceRule::apply_parameters_update() +{ + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + } + // update param values + end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force; + vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_); + vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); + vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); + vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); + + for (size_t i = 0; i < 6; ++i) + { + admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); + } +} + +bool AdmittanceRule::get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) +{ + // get reference transforms + bool success = kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); + + // get transforms at current configuration + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.fixed_world_frame.frame.id, + admittance_transforms_.world_base_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.gravity_compensation.frame.id, + admittance_transforms_.base_cog_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.control.frame.id, + admittance_transforms_.base_control_); + + return success; +} + +// Update from reference joint states +controller_interface::return_type AdmittanceRule::update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) +{ + const double dt = period.seconds(); + + if (parameters_.enable_parameter_update_without_reactivation) + { + apply_parameters_update(); + } + + bool success = get_all_transforms(current_joint_state, reference_joint_state); + + // apply filter and update wrench_world_ vector + Eigen::Matrix rot_world_sensor = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); + Eigen::Matrix rot_world_cog = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation(); + process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); + + // transform wrench_world_ into base frame + admittance_state_.wrench_base.block<3, 1>(0, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0); + admittance_state_.wrench_base.block<3, 1>(3, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0); + + // Compute admittance control law + vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); + admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation(); + admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; + admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; + success &= calculate_admittance_rule(admittance_state_, dt); + + // if a failure occurred during any kinematics interface calls, return an error and don't + // modify the desired reference + if (!success) + { + desired_joint_state = reference_joint_state; + return controller_interface::return_type::ERROR; + } + + // update joint desired joint state + for (size_t i = 0; i < num_joints_; ++i) + { + desired_joint_state.positions[i] = + reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; + desired_joint_state.velocities[i] = + reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + desired_joint_state.accelerations[i] = + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; + } + + return controller_interface::return_type::OK; +} + +bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) +{ + // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness + // correspond to the six diagonal elements of the stiffness matrix expressed in the control frame + auto rot_base_control = admittance_state.rot_base_control; + Eigen::Matrix K = Eigen::Matrix::Zero(); + Eigen::Matrix K_pos = Eigen::Matrix::Zero(); + Eigen::Matrix K_rot = Eigen::Matrix::Zero(); + K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); + K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); + // Transform to the control frame + // A reference is here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf + // Force Control by Luigi Villani and Joris De Schutter + // Page 200 + K_pos = rot_base_control * K_pos * rot_base_control.transpose(); + K_rot = rot_base_control * K_rot * rot_base_control.transpose(); + K.block<3, 3>(0, 0) = K_pos; + K.block<3, 3>(3, 3) = K_rot; + + // The same for damping + Eigen::Matrix D = Eigen::Matrix::Zero(); + Eigen::Matrix D_pos = Eigen::Matrix::Zero(); + Eigen::Matrix D_rot = Eigen::Matrix::Zero(); + D_pos.diagonal() = admittance_state.damping.block<3, 1>(0, 0); + D_rot.diagonal() = admittance_state.damping.block<3, 1>(3, 0); + D_pos = rot_base_control * D_pos * rot_base_control.transpose(); + D_rot = rot_base_control * D_rot * rot_base_control.transpose(); + D.block<3, 3>(0, 0) = D_pos; + D.block<3, 3>(3, 3) = D_rot; + + // calculate admittance relative offset in base frame + Eigen::Isometry3d desired_trans_base_ft; + kinematics_->calculate_link_transform( + admittance_state.current_joint_pos, admittance_state.ft_sensor_frame, desired_trans_base_ft); + Eigen::Matrix X; + X.block<3, 1>(0, 0) = + desired_trans_base_ft.translation() - admittance_state.ref_trans_base_ft.translation(); + auto R_ref = admittance_state.ref_trans_base_ft.rotation(); + auto R_desired = desired_trans_base_ft.rotation(); + auto R = R_desired * R_ref.transpose(); + auto angle_axis = Eigen::AngleAxisd(R); + X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis(); + + // get admittance relative velocity + auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity.data()); + + // external force expressed in the base frame + auto F_base = admittance_state.wrench_base; + + // zero out any forces in the control frame + Eigen::Matrix F_control; + F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0); + F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0); + F_control = F_control.cwiseProduct(admittance_state.selected_axes); + F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0); + F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); + + // Compute admittance control law in the base frame: F = M*x_ddot + D*x_dot + K*x + Eigen::Matrix X_ddot = + admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X); + bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas( + admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame, + admittance_state.joint_acc); + + // add damping if cartesian velocity falls below threshold + for (int64_t i = 0; i < admittance_state.joint_acc.size(); ++i) + { + admittance_state.joint_acc[i] -= + parameters_.admittance.joint_damping * admittance_state.joint_vel[i]; + } + + // integrate motion in joint space + admittance_state.joint_vel += (admittance_state.joint_acc) * dt; + admittance_state.joint_pos += admittance_state.joint_vel * dt; + + // calculate admittance velocity corresponding to joint velocity ("base_link" frame) + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( + admittance_state.current_joint_pos, admittance_state.joint_vel, + admittance_state.ft_sensor_frame, admittance_state.admittance_velocity); + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( + admittance_state.current_joint_pos, admittance_state.joint_acc, + admittance_state.ft_sensor_frame, admittance_state.admittance_acceleration); + + return success; +} + +void AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench & measured_wrench, + const Eigen::Matrix & sensor_world_rot, + const Eigen::Matrix & cog_world_rot) +{ + Eigen::Matrix new_wrench; + new_wrench(0, 0) = measured_wrench.force.x; + new_wrench(1, 0) = measured_wrench.force.y; + new_wrench(2, 0) = measured_wrench.force.z; + new_wrench(0, 1) = measured_wrench.torque.x; + new_wrench(1, 1) = measured_wrench.torque.y; + new_wrench(2, 1) = measured_wrench.torque.z; + + // transform to world frame + Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; + + // apply gravity compensation + new_wrench_base(2, 0) -= end_effector_weight_[2]; + new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); + + // apply smoothing filter + for (size_t i = 0; i < 6; ++i) + { + wrench_world_(i) = filters::exponentialSmoothing( + new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); + } +} + +const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() +{ + for (size_t i = 0; i < parameters_.joints.size(); ++i) + { + state_message_.joint_state.name[i] = parameters_.joints[i]; + state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; + state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; + state_message_.damping.data[i] = admittance_state_.damping[i]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); + state_message_.mass.data[i] = admittance_state_.mass[i]; + } + + state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; + state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1]; + state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2]; + state_message_.wrench_base.wrench.torque.x = admittance_state_.wrench_base[3]; + state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4]; + state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5]; + + state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0]; + state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1]; + state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2]; + state_message_.admittance_velocity.twist.angular.x = admittance_state_.admittance_velocity[3]; + state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4]; + state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5]; + + state_message_.admittance_acceleration.twist.linear.x = + admittance_state_.admittance_acceleration[0]; + state_message_.admittance_acceleration.twist.linear.y = + admittance_state_.admittance_acceleration[1]; + state_message_.admittance_acceleration.twist.linear.z = + admittance_state_.admittance_acceleration[2]; + state_message_.admittance_acceleration.twist.angular.x = + admittance_state_.admittance_acceleration[3]; + state_message_.admittance_acceleration.twist.angular.y = + admittance_state_.admittance_acceleration[4]; + state_message_.admittance_acceleration.twist.angular.z = + admittance_state_.admittance_acceleration[5]; + + state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); + + state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; + state_message_.ref_trans_base_ft.header.frame_id = "ft_reference"; + state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); + + Eigen::Quaterniond quat(admittance_state_.rot_base_control); + state_message_.rot_base_control.w = quat.w(); + state_message_.rot_base_control.x = quat.x(); + state_message_.rot_base_control.y = quat.y(); + state_message_.rot_base_control.z = quat.z(); + + state_message_.ft_sensor_frame.data = + admittance_state_.ft_sensor_frame; // TODO(anyone) remove dynamic allocation here + + return state_message_; +} + +template +void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) +{ + for (auto col = 0; col < matrix.cols(); col++) + { + for (auto row = 0; row < matrix.rows(); row++) + { + matrix(row, col) = data[row + col * matrix.rows()]; + } + } +} + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ diff --git a/admittance_controller/include/admittance_controller/visibility_control.h b/admittance_controller/include/admittance_controller/visibility_control.h new file mode 100644 index 0000000000..24f17a5c2c --- /dev/null +++ b/admittance_controller/include/admittance_controller/visibility_control.h @@ -0,0 +1,49 @@ +// 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. + +#ifndef ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define ADMITTANCE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define ADMITTANCE_CONTROLLER_EXPORT __declspec(dllexport) +#define ADMITTANCE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef ADMITTANCE_CONTROLLER_BUILDING_DLL +#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_EXPORT +#else +#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_IMPORT +#endif +#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE ADMITTANCE_CONTROLLER_PUBLIC +#define ADMITTANCE_CONTROLLER_LOCAL +#else +#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define ADMITTANCE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define ADMITTANCE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define ADMITTANCE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define ADMITTANCE_CONTROLLER_PUBLIC +#define ADMITTANCE_CONTROLLER_LOCAL +#endif +#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml new file mode 100644 index 0000000000..e352dcfa91 --- /dev/null +++ b/admittance_controller/package.xml @@ -0,0 +1,43 @@ + + + + admittance_controller + 0.0.0 + Implementation of admittance controllers for different input and output interface. + Denis Štogl + Andy Zelenak + Apache License 2.0 + + ament_cmake + + control_msgs + control_toolbox + controller_interface + kinematics_interface + filters + generate_parameter_library + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs + + + ament_cmake_gmock + control_msgs + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp new file mode 100644 index 0000000000..1af4e1317f --- /dev/null +++ b/admittance_controller/src/admittance_controller.cpp @@ -0,0 +1,592 @@ +// Copyright (c) 2022, 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. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#include "admittance_controller/admittance_controller.hpp" + +#include +#include +#include +#include +#include +#include + +#include "admittance_controller/admittance_rule_impl.hpp" +#include "geometry_msgs/msg/wrench.hpp" +#include "rcutils/logging_macros.h" +#include "tf2_ros/buffer.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace admittance_controller +{ +controller_interface::CallbackReturn AdmittanceController::on_init() +{ + // initialize controller config + try + { + parameter_handler_ = std::make_shared(get_node()); + admittance_ = std::make_unique(parameter_handler_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // number of joints in controllers is fixed after initialization + num_joints_ = admittance_->parameters_.joints.size(); + + // allocate dynamic memory + last_reference_.positions.assign(num_joints_, 0.0); + last_reference_.velocities.assign(num_joints_, 0.0); + last_reference_.accelerations.assign(num_joints_, 0.0); + last_commanded_ = last_reference_; + reference_ = last_reference_; + reference_admittance_ = last_reference_; + joint_state_ = last_reference_; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() + const +{ + std::vector command_interfaces_config_names; + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + for (const auto & joint : command_joint_names_) + { + auto full_name = joint + "/" + interface; + command_interfaces_config_names.push_back(full_name); + } + } + + return { + controller_interface::interface_configuration_type::INDIVIDUAL, + command_interfaces_config_names}; +} + +controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() + const +{ + std::vector state_interfaces_config_names; + for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) + { + const auto & interface = admittance_->parameters_.state_interfaces[i]; + for (const auto & joint : admittance_->parameters_.joints) + { + auto full_name = joint + "/" + interface; + state_interfaces_config_names.push_back(full_name); + } + } + + auto ft_interfaces = force_torque_sensor_->get_state_interface_names(); + state_interfaces_config_names.insert( + state_interfaces_config_names.end(), ft_interfaces.begin(), ft_interfaces.end()); + + return { + controller_interface::interface_configuration_type::INDIVIDUAL, state_interfaces_config_names}; +} + +std::vector +AdmittanceController::on_export_reference_interfaces() +{ + // create CommandInterface interfaces that other controllers will be able to chain with + if (!admittance_) + { + return {}; + } + + 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_ = {}; + + // assign reference interfaces + auto index = 0ul; + for (const auto & interface : allowed_reference_interfaces_types_) + { + for (const auto & joint : admittance_->parameters_.joints) + { + if (hardware_interface::HW_IF_POSITION == interface) + position_reference_.emplace_back(reference_interfaces_[index]); + else if (hardware_interface::HW_IF_VELOCITY == interface) + { + velocity_reference_.emplace_back(reference_interfaces_[index]); + } + 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++; + } + } + + return chainable_command_interfaces; +} + +controller_interface::CallbackReturn AdmittanceController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + try + { + parameter_handler_ = std::make_shared(get_node()); + admittance_ = std::make_unique(parameter_handler_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + command_joint_names_ = admittance_->parameters_.command_joints; + if (command_joint_names_.empty()) + { + command_joint_names_ = admittance_->parameters_.joints; + RCLCPP_INFO( + get_node()->get_logger(), + "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != num_joints_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + + // print and validate interface types + for (const auto & tmp : admittance_->parameters_.state_interfaces) + { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); + } + for (const auto & tmp : admittance_->parameters_.command_interfaces) + { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); + } + + // validate exported interfaces + for (const auto & tmp : admittance_->parameters_.chainable_command_interfaces) + { + if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_INFO( + get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "chainable interface type %s is not supported. Supported types are %s and %s", tmp.c_str(), + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); + return controller_interface::CallbackReturn::ERROR; + } + } + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + auto contains_interface_type = + [](const std::vector & interface_type_list, const std::string & interface_type) + { + return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != + interface_type_list.end(); + }; + + joint_command_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + 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 '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } + } + + has_position_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_ACCELERATION); + has_effort_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_EFFORT); + + // 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 : admittance_->parameters_.state_interfaces) + { + 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 '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } + } + + has_position_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); + + 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(); + }; + RCLCPP_INFO( + get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", + get_interface_list(admittance_->parameters_.command_interfaces).c_str(), + get_interface_list(admittance_->parameters_.state_interfaces).c_str()); + + // setup subscribers and publishers + auto joint_command_callback = + [this](const std::shared_ptr msg) + { input_joint_command_.writeFromNonRT(msg); }; + input_joint_command_subscriber_ = + get_node()->create_subscription( + "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); + s_publisher_ = get_node()->create_publisher( + "~/status", rclcpp::SystemDefaultsQoS()); + state_publisher_ = + std::make_unique>(s_publisher_); + + // Initialize state message + state_publisher_->lock(); + state_publisher_->msg_ = admittance_->get_controller_state(); + state_publisher_->unlock(); + + // Initialize FTS semantic semantic_component + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); + + // configure admittance rule + if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + { + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // on_activate is called when the lifecycle node activates. + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + + // order all joints in the storage + for (const auto & interface : admittance_->parameters_.state_interfaces) + { + 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_, admittance_->parameters_.joints, interface, + joint_state_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_, + interface.c_str(), joint_state_interface_[index].size()); + return CallbackReturn::ERROR; + } + } + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + 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_, command_joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_, + interface.c_str(), joint_command_interface_[index].size()); + return CallbackReturn::ERROR; + } + } + + // update parameters if any have changed + admittance_->apply_parameters_update(); + + // initialize interface of the FTS semantic component + force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); + + // initialize states + read_state_from_hardware(joint_state_, ft_values_); + for (auto val : joint_state_.positions) + { + if (std::isnan(val)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read joint positions from the hardware.\n"); + return controller_interface::CallbackReturn::ERROR; + } + } + + // Use current joint_state as a default reference + last_reference_ = joint_state_; + last_commanded_ = joint_state_; + reference_ = joint_state_; + reference_admittance_ = joint_state_; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type AdmittanceController::update_reference_from_subscribers() +{ + // update input reference from ros subscriber message + if (!admittance_) + { + return controller_interface::return_type::ERROR; + } + + joint_command_msg_ = *input_joint_command_.readFromRT(); + + // if message exists, load values into references + if (joint_command_msg_.get()) + { + for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + { + 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]; + } + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + // Realtime constraints are required in this function + if (!admittance_) + { + return controller_interface::return_type::ERROR; + } + + // update input reference from chainable interfaces + read_state_reference_interfaces(reference_); + + // get all controller inputs + read_state_from_hardware(joint_state_, ft_values_); + + // apply admittance control to reference to determine desired state + admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + + // write calculated values to joint interfaces + write_state_to_hardware(reference_admittance_); + + // Publish controller state + state_publisher_->lock(); + state_publisher_->msg_ = admittance_->get_controller_state(); + state_publisher_->unlockAndPublish(); + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn AdmittanceController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + + // release force torque sensor interface + force_torque_sensor_->release_interfaces(); + + // 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 (size_t index = 0; index < allowed_interface_types_.size(); ++index) + { + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } + release_interfaces(); + admittance_->reset(num_joints_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_error( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + admittance_->reset(num_joints_); + return controller_interface::CallbackReturn::SUCCESS; +} + +void AdmittanceController::read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state_current, + geometry_msgs::msg::Wrench & ft_values) +{ + // if any interface has nan values, assume state_current is the last command state + bool nan_position = false; + bool nan_velocity = false; + bool nan_acceleration = false; + + 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_; + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) + { + if (has_position_state_interface_) + { + state_current.positions[joint_ind] = + 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(); + 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(); + nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]); + } + } + + if (nan_position) + { + state_current.positions = last_commanded_.positions; + } + if (nan_velocity) + { + state_current.velocities = last_commanded_.velocities; + } + if (nan_acceleration) + { + state_current.accelerations = last_commanded_.accelerations; + } + + // if any ft_values are nan, assume values are zero + force_torque_sensor_->get_values_as_message(ft_values); + if ( + std::isnan(ft_values.force.x) || std::isnan(ft_values.force.y) || + std::isnan(ft_values.force.z) || std::isnan(ft_values.torque.x) || + std::isnan(ft_values.torque.y) || std::isnan(ft_values.torque.z)) + { + ft_values = geometry_msgs::msg::Wrench(); + } +} + +void AdmittanceController::write_state_to_hardware( + const trajectory_msgs::msg::JointTrajectoryPoint & state_commanded) +{ + // 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_; + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) + { + if (has_position_command_interface_) + { + command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + else if (has_velocity_command_interface_) + { + command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + else if (has_acceleration_command_interface_) + { + command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + } + last_commanded_ = state_commanded; +} + +void AdmittanceController::read_state_reference_interfaces( + trajectory_msgs::msg::JointTrajectoryPoint & state_reference) +{ + // TODO(destogl): check why is this here? + + // 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])) + { + 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]; + } + state_reference.velocities[i] = velocity_reference_[i]; + } + + last_reference_.positions = state_reference.positions; + last_reference_.velocities = state_reference.velocities; +} + +} // namespace admittance_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + admittance_controller::AdmittanceController, controller_interface::ChainableControllerInterface) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml new file mode 100644 index 0000000000..5bdc40e398 --- /dev/null +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -0,0 +1,159 @@ +admittance_controller: + joints: { + type: string_array, + description: "Specifies which joints will be used by the controller. ", + read_only: true + } + command_joints: { + type: string_array, + default_value: [], + description: "(optional) Specifies the joints for writing into another controllers reference. This parameter is only relevant when chaining the output of this controller to the input of another controller.", + read_only: true + } + command_interfaces: + { + type: string_array, + description: "Specifies which command interfaces the controller will claim.", + read_only: true + } + + state_interfaces: + { + type: string_array, + description: "Specifies which state interfaces the controller will claim.", + read_only: true + } + + chainable_command_interfaces: + { + type: string_array, + description: "Specifies which reference interfaces the controller will export. Normally, the position and velocity are used.", + read_only: true + } + + kinematics: + plugin_name: { + type: string, + description: "Specifies the name of the kinematics plugin to load." + } + plugin_package: { + type: string, + description: "Specifies the package name that contains the kinematics plugin." + } + base: { + type: string, + description: "Specifies the base link of the robot description used by the kinematics plugin." + } + tip: { + type: string, + description: "Specifies the end effector link of the robot description used by the kinematics plugin." + } + alpha: { + type: double, + default_value: 0.01, + description: "Specifies the damping coefficient for the Jacobian pseudo inverse." + } + + ft_sensor: + name: { + type: string, + description: "Specifies the name of the force torque sensor in the robot description which will be used in the admittance calculation." + } + frame: + id: { + type: string, + description: "Specifies the frame/link name of the force torque sensor." + } + filter_coefficient: { + type: double, + default_value: 0.05, + description: "Specifies the filter coefficient for the sensor's exponential filter." + } + + control: + frame: + id: { + type: string, + description: "Specifies the control frame used for admittance calculation." + } + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: { + type: string, + description: "Specifies the world frame use for admittance calculation. Gravity must point down in this frame." + } + + gravity_compensation: + frame: + id: { + type: string, + description: "Specifies the frame which center of gravity (CoG) is defined in. Normally, the force torque sensor frame should be used." + } + CoG: + pos: { + type: double_array, + description: "Specifies the position of the center of gravity (CoG) of the end effector in the gravity compensation frame.", + validation: { + fixed_size<>: 3 + } + } + force: { + type: double, + default_value: 0.0, + description: "Specifies the weight of the end effector, e.g mass * 9.81." + } + + admittance: + selected_axes: + { + type: bool_array, + description: "Specifies whether the axes x, y, z, rx, ry, and rz should be included in the admittance calculation.", + validation: { + fixed_size<>: 6 + } + } + mass: { + type: double_array, + description: "Specifies the mass values for x, y, z, rx, ry, and rz used in the admittance calculation.", + validation: { + fixed_size<>: 6, + element_bounds<>: [ 0.0001, 1000000.0 ] + } + } + damping_ratio: { + type: double_array, + description: "Specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. + The damping ratio is defined as: zeta = D / (2 * sqrt( M * S )).", + validation: { + fixed_size<>: 6 + } + } + stiffness: { + type: double_array, + description: "Specifies the stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation.", + validation: { + fixed_size<>: 6, + element_bounds<>: [ 0.0, 100000000.0 ] + } + } + joint_damping: { + type: double, + description: "Specifies the joint damping applied used in the admittance calculation.", + default_value: 5, + validation: { + lower_bounds: [ 0.0 ] + } + } + + # general settings + robot_description: { + type: string, + description: "Contains robot description in URDF format. The description is used for forward and inverse kinematics.", + read_only: true + } + enable_parameter_update_without_reactivation: { + type: bool, + default_value: true, + description: "If enabled, the parameters will be dynamically updated while the controller is running." + } diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/6d_robot_description.hpp new file mode 100644 index 0000000000..f67b5bd054 --- /dev/null +++ b/admittance_controller/test/6d_robot_description.hpp @@ -0,0 +1,313 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ + +#include + +namespace ros2_control_test_assets +{ +const auto valid_6d_robot_urdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ros2_control_kuka_demo_driver/KukaSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + +)"; + +const auto valid_6d_robot_srdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace ros2_control_test_assets + +#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp new file mode 100644 index 0000000000..d056e0406c --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -0,0 +1,292 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "test_admittance_controller.hpp" + +#include +#include +#include +#include + +// Test on_configure returns ERROR when a required parameter is missing +TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) +{ + ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, + 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", + "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", + "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + +INSTANTIATE_TEST_SUITE_P( + InvalidParameterDuringConfiguration, AdmittanceControllerTestParameterizedInvalidParameters, + ::testing::Values( + // wrong length COG + std::make_tuple( + std::string("gravity_compensation.CoG.pos"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3, 4})), + // wrong length stiffness + std::make_tuple( + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // negative stiffness + std::make_tuple( + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), + // wrong length mass + std::make_tuple( + std::string("admittance.mass"), rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // negative mass + std::make_tuple( + std::string("admittance.mass"), + rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), + // wrong length damping ratio + std::make_tuple( + std::string("admittance.damping_ratio"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // wrong length selected axes + std::make_tuple( + std::string("admittance.selected_axes"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // invalid robot description + std::make_tuple( + std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))); + +TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) +{ + auto result = SetUpController(); + + ASSERT_EQ(result, controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(!controller_->admittance_->parameters_.joints.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.joints.size() == joint_names_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.joints.begin(), + controller_->admittance_->parameters_.joints.end(), joint_names_.begin(), joint_names_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.command_interfaces.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.command_interfaces.size() == + command_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.command_interfaces.begin(), + controller_->admittance_->parameters_.command_interfaces.end(), + command_interface_types_.begin(), command_interface_types_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.state_interfaces.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.state_interfaces.size() == state_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.state_interfaces.begin(), + controller_->admittance_->parameters_.state_interfaces.end(), state_interface_types_.begin(), + state_interface_types_.end())); + + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); + ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.frame.id, sensor_frame_); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.selected_axes.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.selected_axes.size() == + admittance_selected_axes_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.selected_axes.begin(), + controller_->admittance_->parameters_.admittance.selected_axes.end(), + admittance_selected_axes_.begin(), admittance_selected_axes_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.mass.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.mass.size() == admittance_mass_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.mass.begin(), + controller_->admittance_->parameters_.admittance.mass.end(), admittance_mass_.begin(), + admittance_mass_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.damping_ratio.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.damping_ratio.size() == + admittance_damping_ratio_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.damping_ratio.begin(), + controller_->admittance_->parameters_.admittance.damping_ratio.end(), + admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.stiffness.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.stiffness.size() == + admittance_stiffness_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.stiffness.begin(), + controller_->admittance_->parameters_.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); +} + +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(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + 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(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + 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(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // // Check that wrench command are all zero since not used + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); + // ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); + // ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); + // ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); + // ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); + // ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); + + // // Check joint command message + // for (auto i = 0ul; i < joint_names_.size(); i++) + // { + // ASSERT_EQ(joint_names_[i], msg.joint_state.name[i]); + // ASSERT_FALSE(std::isnan(msg.joint_state.position[i])); + // ASSERT_FALSE(std::isnan(msg.joint_state.velocity[i])); + // ASSERT_FALSE(std::isnan(msg.joint_state.effort[i])); + // } +} + +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(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + 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++) + { + ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); + } + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + broadcast_tfs(); + ASSERT_EQ( + 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); + + subscribe_and_get_messages(msg); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} + +// 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..be78f05bb9 --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -0,0 +1,463 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 TEST_ADMITTANCE_CONTROLLER_HPP_ +#define TEST_ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "6d_robot_description.hpp" +#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_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "semantic_components/force_torque_sensor.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; +using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; +using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectoryPoint; +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +namespace +{ +const double COMMON_THRESHOLD = 0.001; + +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 variables +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); + FRIEND_TEST(AdmittanceControllerTest, receive_message_and_publish_updated_status); + +public: + CallbackReturn on_init() override + { + get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->declare_parameter( + "robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description", robot_description_}); + get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); + + return admittance_controller::AdmittanceController::on_init(); + } + + 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_pose_command_subscriber_wait_set_.add_subscription(input_joint_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_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + + if (success) + { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; + rclcpp::WaitSet input_pose_command_subscriber_wait_set_; + const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; +}; + +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_references", rclcpp::SystemDefaultsQoS()); + // pose_command_publisher_ =command_publisher_node_->create_publisher( + // "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); + joint_command_publisher_ = command_publisher_node_->create_publisher( + "/test_admittance_controller/joint_references", 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: + controller_interface::return_type SetUpController( + const std::string & controller_name, const std::vector & parameter_overrides) + { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(false) + .parameter_overrides(parameter_overrides) + .automatically_declare_parameters_from_overrides(true); + return SetUpControllerCommon(controller_name, options); + } + + controller_interface::return_type SetUpController( + const std::string & controller_name = "test_admittance_controller") + { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(false) + .automatically_declare_parameters_from_overrides(true); + return SetUpControllerCommon(controller_name, options); + } + + controller_interface::return_type SetUpControllerCommon( + const std::string & controller_name, const rclcpp::NodeOptions & options) + { + auto result = controller_->init(controller_name, "", options); + + controller_->export_reference_interfaces(); + assign_interfaces(); + + return result; + } + + 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 = fixed_world_frame_; + transform_stamped.transform.translation.x = 1.3; + transform_stamped.transform.translation.y = 0.5; + transform_stamped.transform.translation.z = 0.5; + 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 = ik_base_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = ik_tip_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.header.frame_id = ik_tip_frame_; + transform_stamped.transform.translation.x = 0; + transform_stamped.transform.translation.y = 0; + transform_stamped.transform.translation.z = 0; + 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 = control_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.05; + transform_stamped.child_frame_id = sensor_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.2; + transform_stamped.child_frame_id = endeffector_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/status", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + 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; + } + }; + + // TODO(destogl): comment in when using unified mode + // if (controller_->admittance_->unified_mode_) { + // wait_for_topic(force_command_publisher_->get_topic_name()); + // } + + // wait_for_topic(pose_command_publisher_->get_topic_name()); + + ControllerCommandWrenchMsg 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; + + // TODO(destogl): comment in when using unified mode + // if (controller_->admittance_->unified_mode_) { + // force_command_publisher_->publish(force_msg); + // } + // pose_command_publisher_->publish(pose_msg); + + wait_for_topic(joint_command_publisher_->get_topic_name()); + + ControllerCommandJointMsg joint_msg; + // joint_msg.joint_names = joint_names_; + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; + auto num_joints = joint_names_.size(); + trajectory_point.positions.reserve(num_joints); + 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]); + } + joint_msg = trajectory_point; + + joint_command_publisher_->publish(joint_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"; + + bool hardware_state_has_offset_ = false; + + const std::string ik_base_frame_ = "base_link"; + const std::string ik_tip_frame_ = "tool0"; + const std::string ik_group_name_ = "arm"; + // const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + // const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; + + const std::string control_frame_ = "tool0"; + const std::string endeffector_frame_ = "endeffector_frame"; + const std::string fixed_world_frame_ = "fixed_world_frame"; + const std::string sensor_frame_ = "link_6"; + + 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_ratio_ = {2.828427, 2.828427, 2.828427, + 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}; + 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::Publisher::SharedPtr joint_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 AdmittanceControllerTestParameterizedMissingParameters +: public AdmittanceControllerTest, + public ::testing::WithParamInterface +{ +public: + virtual void SetUp() + { + AdmittanceControllerTest::SetUp(); + auto node = std::make_shared("test_admittance_controller"); + overrides_ = node->get_node_parameters_interface()->get_parameter_overrides(); + } + + static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } + +protected: + controller_interface::return_type SetUpController(const std::string & remove_name) + { + std::vector parameter_overrides; + for (const auto & override : overrides_) + { + if (override.first != remove_name) + { + rclcpp::Parameter param(override.first, override.second); + parameter_overrides.push_back(param); + } + } + + return AdmittanceControllerTest::SetUpController( + "test_admittance_controller_no_overrides", parameter_overrides); + } + + std::map overrides_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class AdmittanceControllerTestParameterizedInvalidParameters +: public AdmittanceControllerTest, + public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() { AdmittanceControllerTest::SetUp(); } + + static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } + +protected: + void SetUpController() + { + AdmittanceControllerTest::SetUpController("test_admittance_controller"); + } +}; + +#endif // TEST_ADMITTANCE_CONTROLLER_HPP_ diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp new file mode 100644 index 0000000000..1f290aeff6 --- /dev/null +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -0,0 +1,47 @@ +// 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. + +#include + +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadAdmittanceController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController")); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml new file mode 100644 index 0000000000..22cbda2df9 --- /dev/null +++ b/admittance_controller/test/test_params.yaml @@ -0,0 +1,111 @@ +load_admittance_controller: + # contains minimal parameters that need to be set to load controller + ros__parameters: + joints: + - joint1 + - joint2 + + command_interfaces: + - velocity + + state_interfaces: + - position + - velocity + + chainable_command_interfaces: + - position + - velocity + + +test_admittance_controller: + # contains minimal needed parameters for kuka_kr6 + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + command_interfaces: + - position + + state_interfaces: + - position + + chainable_command_interfaces: + - position + - velocity + + kinematics: + plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL + plugin_package: kinematics_interface + base: base_link # Assumed to be stationary + tip: tool0 + group_name: arm + alpha: 0.0005 + + ft_sensor: + name: ft_sensor_name + frame: + id: link_6 # tool0 Wrench measurements are in this frame + external: false # force torque frame exists within URDF kinematic chain + filter_coefficient: 0.005 + + control: + frame: + id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + gravity_compensation: + frame: + id: tool0 + external: false + + CoG: # specifies the center of gravity of the end effector + pos: + - 0.1 # x + - 0.0 # y + - 0.0 # z + force: 23.0 # mass * 9.81 + + admittance: + selected_axes: + - true # x + - true # y + - true # z + - true # rx + - true # ry + - true # rz + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + - 5.5 + - 6.6 + - 7.7 + - 8.8 + - 9.9 + - 10.10 + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + - 2.828427 # x + - 2.828427 # y + - 2.828427 # z + - 2.828427 # rx + - 2.828427 # ry + - 2.828427 # rz + + stiffness: + - 214.1 + - 214.2 + - 214.3 + - 214.4 + - 214.5 + - 214.6 diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 1b3910e7e7..b9cdead153 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 1b3910e7e7..b9cdead153 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index 09ec6e5387..d9a1c841a5 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -11,7 +11,3 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 2011f0c31a..b9b8674fc9 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -11,7 +11,3 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 3a667e7d9a..8c0ba1ff8a 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -3,11 +3,19 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 6f9dc84a3f..f62ba87bf3 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -3,15 +3,20 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: master - generate_parameter_library: + control_msgs: type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + url: https://github.com/ros-controls/control_msgs.git + version: humble + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master