diff --git a/force_gate_controller/CMakeLists.txt b/force_gate_controller/CMakeLists.txt index d5e5306..4722886 100644 --- a/force_gate_controller/CMakeLists.txt +++ b/force_gate_controller/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.16) project(force_gate_controller LANGUAGES CXX) -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wconversion) endif() @@ -12,13 +12,16 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library hardware_interface + moveit_ros_control_interface pluginlib + position_controllers rclcpp rclcpp_lifecycle realtime_tools rsl tl_expected trajectory_msgs + velocity_controllers ) find_package(ament_cmake REQUIRED) @@ -34,7 +37,13 @@ generate_parameter_library(force_gate_controller_parameters add_library(force_gate_controller SHARED src/force_gate_controller.cpp + src/force_gate_parent.cpp + src/force_gate_position_controller.cpp + src/force_gate_velocity_controller.cpp + src/joint_trajectory_controller_plugin.cpp + src/tolerances.cpp src/trajectory.cpp + src/validate_jtc_parameters.cpp ) target_compile_features(force_gate_controller PUBLIC cxx_std_17) target_include_directories(force_gate_controller PUBLIC @@ -49,7 +58,8 @@ ament_target_dependencies(force_gate_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DE # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(force_gate_controller PRIVATE "FORCE_GATE_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") -pluginlib_export_plugin_description_file(controller_interface force_gate_plugin.xml) +pluginlib_export_plugin_description_file(controller_interface force_gate_controller_plugin.xml) +pluginlib_export_plugin_description_file(moveit_ros_control_interface force_gate_controller_allocator_plugin.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) diff --git a/force_gate_controller/force_gate_controller_allocator_plugin.xml b/force_gate_controller/force_gate_controller_allocator_plugin.xml new file mode 100644 index 0000000..97cd45b --- /dev/null +++ b/force_gate_controller/force_gate_controller_allocator_plugin.xml @@ -0,0 +1,7 @@ + + + + A ControllerAllocator for the force gates joint trajectory controller, to enable it to be controlled by MoveIt's Ros2ControlManager. + + + diff --git a/force_gate_controller/force_gate_controller_plugin.xml b/force_gate_controller/force_gate_controller_plugin.xml new file mode 100644 index 0000000..3b884c8 --- /dev/null +++ b/force_gate_controller/force_gate_controller_plugin.xml @@ -0,0 +1,21 @@ + + + + The joint trajectory controller executes joint-space trajectories on a set of joints and allows for Force / Torque safety thresholding. + + + + + + + The joint group velocity controller executes joint-space trajectories on a set of joints and allows for Force / Torque safety thresholding. + + + + + + + The joint group position controller executes joint-space trajectories on a set of joints and allows for Force / Torque safety thresholding. + + + diff --git a/force_gate_controller/force_gate_plugin.xml b/force_gate_controller/force_gate_plugin.xml deleted file mode 100644 index 2b6c19f..0000000 --- a/force_gate_controller/force_gate_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The joint trajectory controller executes joint-space trajectories on a set of joints and allows for Force / Torque safety thresholding. - - - diff --git a/force_gate_controller/include/force_gate_controller/force_gate_parent.hpp b/force_gate_controller/include/force_gate_controller/force_gate_parent.hpp new file mode 100644 index 0000000..5de3708 --- /dev/null +++ b/force_gate_controller/include/force_gate_controller/force_gate_parent.hpp @@ -0,0 +1,52 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 FORCE_GATE_CONTROLLER__FORCE_GATE_PARENT_HPP_ +#define FORCE_GATE_CONTROLLER__FORCE_GATE_PARENT_HPP_ + +// #include + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "force_gate_controller/tolerances.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "realtime_tools/realtime_buffer.h" + +// auto-generated by generate_parameter_library +#include "force_gate_controller_parameters.hpp" + +namespace force_gate_controller +{ +/** + * \brief Defines shared functions and attributes to force-gate a controller. + */ +class ForceGateParent +{ +protected: + controller_interface::CallbackReturn read_force_gate_parameters(std::shared_ptr node); + + WrenchTolerances wrench_tolerances_; + realtime_tools::RealtimeBuffer> rt_wrench_stamped_; + rclcpp::Subscription::SharedPtr wrench_subscriber_ = nullptr; + + // Parameters from ROS for force_gate_controller + std::shared_ptr force_gate_param_listener_; + Params force_gate_params_; + + bool check_wrench_threshold(std::shared_ptr node, const rclcpp::Time & time); +}; + +} // namespace force_gate_controller + +#endif // FORCE_GATE_CONTROLLER__FORCE_GATE_PARENT_HPP_ diff --git a/force_gate_controller/include/force_gate_controller/force_gate_position_controller.hpp b/force_gate_controller/include/force_gate_controller/force_gate_position_controller.hpp new file mode 100644 index 0000000..c06f2b0 --- /dev/null +++ b/force_gate_controller/include/force_gate_controller/force_gate_position_controller.hpp @@ -0,0 +1,60 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 FORCE_GATE_CONTROLLER__FORCE_GATE_POSITION_CONTROLLER_HPP_ +#define FORCE_GATE_CONTROLLER__FORCE_GATE_POSITION_CONTROLLER_HPP_ + +// #include + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "position_controllers/joint_group_position_controller.hpp" +#include "force_gate_controller/force_gate_parent.hpp" +#include "force_gate_controller/visibility_control.h" +#include "force_gate_controller/tolerances.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +namespace force_gate_controller +{ +/** + * \brief A force-gated version of the JointGroupPositionController. + * + * Forward command controller for a set of position controlled joints (linear or angular). + * + * This class forwards the commanded positions down to a set of joints. + * + * \param joints Names of the joints to control. + * + * Subscribes to: + * - \b commands (std_msgs::msg::Float64MultiArray) : The position commands to apply. + */ +class ForceGatePositionController : public position_controllers::JointGroupPositionController, public ForceGateParent +{ +public: + FORCE_GATE_CONTROLLER_PUBLIC + ForceGatePositionController(); + + FORCE_GATE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + FORCE_GATE_CONTROLLER_PUBLIC + controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + // Overridden functions to read parameters from ROS. + controller_interface::CallbackReturn read_parameters() override; +}; + +} // namespace force_gate_controller + +#endif // FORCE_GATE_CONTROLLER__FORCE_GATE_POSITION_CONTROLLER_HPP_ diff --git a/force_gate_controller/include/force_gate_controller/force_gate_velocity_controller.hpp b/force_gate_controller/include/force_gate_controller/force_gate_velocity_controller.hpp new file mode 100644 index 0000000..3f46dc9 --- /dev/null +++ b/force_gate_controller/include/force_gate_controller/force_gate_velocity_controller.hpp @@ -0,0 +1,58 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 FORCE_GATE_CONTROLLER__FORCE_GATE_VELOCITY_CONTROLLER_HPP_ +#define FORCE_GATE_CONTROLLER__FORCE_GATE_VELOCITY_CONTROLLER_HPP_ + +// #include + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "velocity_controllers/joint_group_velocity_controller.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "force_gate_controller/force_gate_parent.hpp" +#include "force_gate_controller/tolerances.hpp" +#include "force_gate_controller/visibility_control.h" + +namespace force_gate_controller +{ +/** + * \brief Forward command controller for a set of velocity controlled joints (linear or angular). + * + * This class forwards the commanded velocities down to a set of joints. + * + * \param joints Names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::msg::Float64MultiArray) : The velocity commands to apply. + */ +class ForceGateVelocityController : public velocity_controllers::JointGroupVelocityController, public ForceGateParent +{ +public: + FORCE_GATE_CONTROLLER_PUBLIC + ForceGateVelocityController(); + + FORCE_GATE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + FORCE_GATE_CONTROLLER_PUBLIC + controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + // Overridden functions to read parameters from ROS. + controller_interface::CallbackReturn read_parameters() override; +}; + +} // namespace force_gate_controller + +#endif // FORCE_GATE_CONTROLLER__FORCE_GATE_VELOCITY_CONTROLLER_HPP_ diff --git a/force_gate_controller/include/force_gate_controller/tolerances.hpp b/force_gate_controller/include/force_gate_controller/tolerances.hpp index 8e0d4f2..94034d9 100644 --- a/force_gate_controller/include/force_gate_controller/tolerances.hpp +++ b/force_gate_controller/include/force_gate_controller/tolerances.hpp @@ -105,33 +105,7 @@ struct SegmentTolerances * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances(Params const & params) -{ - auto const & constraints = params.constraints; - auto const n_joints = params.joints.size(); - - SegmentTolerances tolerances; - tolerances.goal_time_tolerance = constraints.goal_time; - - // State and goal state tolerances - tolerances.state_tolerance.resize(n_joints); - tolerances.goal_state_tolerance.resize(n_joints); - for (size_t i = 0; i < n_joints; ++i) - { - auto const joint = params.joints[i]; - tolerances.state_tolerance[i].position = constraints.joints_map.at(joint).trajectory; - tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; - tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; - - auto logger = rclcpp::get_logger("tolerance"); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); - } - - return tolerances; -} +SegmentTolerances get_segment_tolerances(Params const & params); /** * \param state_error State error to check. @@ -140,54 +114,9 @@ SegmentTolerances get_segment_tolerances(Params const & params) * \param show_errors If the joint that violate its tolerance should be output to console. NOT * REALTIME if true \return True if \p state_error fulfills \p state_tolerance. */ -inline bool check_state_tolerance_per_joint( +bool check_state_tolerance_per_joint( const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx, - const StateTolerances & state_tolerance, bool show_errors = false) -{ - using std::abs; - const double error_position = state_error.positions[joint_idx]; - const double error_velocity = - state_error.velocities.empty() ? 0.0 : state_error.velocities[joint_idx]; - const double error_acceleration = - state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx]; - - const bool is_valid = - !(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) && - !(state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) && - !(state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration); - - if (is_valid) - { - return true; - } - - if (show_errors) - { - const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "Path state tolerances failed:"); - - if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) - { - RCLCPP_ERROR( - logger, "Position Error: %f, Position Tolerance: %f", error_position, - state_tolerance.position); - } - if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) - { - RCLCPP_ERROR( - logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity, - state_tolerance.velocity); - } - if ( - state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration) - { - RCLCPP_ERROR( - logger, "Acceleration Error: %f, Acceleration Tolerance: %f", error_acceleration, - state_tolerance.acceleration); - } - } - return false; -} + const StateTolerances & state_tolerance, bool show_errors); } // namespace force_gate_controller diff --git a/force_gate_controller/include/force_gate_controller/validate_jtc_parameters.hpp b/force_gate_controller/include/force_gate_controller/validate_jtc_parameters.hpp index 521ec5a..23f0859 100644 --- a/force_gate_controller/include/force_gate_controller/validate_jtc_parameters.hpp +++ b/force_gate_controller/include/force_gate_controller/validate_jtc_parameters.hpp @@ -25,74 +25,10 @@ namespace force_gate_controller { tl::expected command_interface_type_combinations( - rclcpp::Parameter const & parameter) -{ - auto const & interface_types = parameter.as_string_array(); - - // Check if command interfaces combination is valid. Valid combinations are: - // 1. effort - // 2. velocity - // 2. position [velocity, [acceleration]] - - if ( - rsl::contains>(interface_types, "velocity") && - interface_types.size() > 1 && - !rsl::contains>(interface_types, "position")) - { - return tl::make_unexpected( - "'velocity' command interface can be used either alone or 'position' " - "interface has to be present"); - } - - if ( - rsl::contains>(interface_types, "acceleration") && - (!rsl::contains>(interface_types, "velocity") && - !rsl::contains>(interface_types, "position"))) - { - return tl::make_unexpected( - "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); - } - - if ( - rsl::contains>(interface_types, "effort") && - interface_types.size() > 1) - { - return tl::make_unexpected("'effort' command interface has to be used alone"); - } - - return {}; -} + rclcpp::Parameter const & parameter); tl::expected state_interface_type_combinations( - rclcpp::Parameter const & parameter) -{ - auto const & interface_types = parameter.as_string_array(); - - // Valid combinations are - // 1. position [velocity, [acceleration]] - - if ( - rsl::contains>(interface_types, "velocity") && - !rsl::contains>(interface_types, "position")) - { - return tl::make_unexpected( - "'velocity' state interface cannot be used if 'position' interface " - "is missing."); - } - - if ( - rsl::contains>(interface_types, "acceleration") && - (!rsl::contains>(interface_types, "position") || - !rsl::contains>(interface_types, "velocity"))) - { - return tl::make_unexpected( - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - } - - return {}; -} + rclcpp::Parameter const & parameter); } // namespace force_gate_controller diff --git a/force_gate_controller/package.xml b/force_gate_controller/package.xml index a06ca30..93501a8 100644 --- a/force_gate_controller/package.xml +++ b/force_gate_controller/package.xml @@ -23,11 +23,14 @@ control_toolbox generate_parameter_library hardware_interface + moveit_ros_control_interface + position_controllers rclcpp rclcpp_lifecycle rsl tl_expected trajectory_msgs + velocity_controllers ament_cmake_gmock controller_manager diff --git a/force_gate_controller/src/force_gate_parent.cpp b/force_gate_controller/src/force_gate_parent.cpp new file mode 100644 index 0000000..85a0044 --- /dev/null +++ b/force_gate_controller/src/force_gate_parent.cpp @@ -0,0 +1,155 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 "controller_interface/controller_interface.hpp" +// #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "force_gate_controller/force_gate_parent.hpp" +#include "rclcpp/logging.hpp" +// #include "rclcpp/parameter.hpp" + +namespace force_gate_controller +{ + +controller_interface::CallbackReturn ForceGateParent::read_force_gate_parameters(std::shared_ptr node) +{ + if (!force_gate_param_listener_) + { + force_gate_param_listener_ = std::make_shared(node); + if (!force_gate_param_listener_) { + RCLCPP_ERROR(node->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + } + force_gate_params_ = force_gate_param_listener_->get_params(); + + // Update wrench tolerances if thresholding enabled + rt_wrench_stamped_.writeFromNonRT(nullptr); + if (force_gate_params_.wrench_threshold.topic != "") + { + RCLCPP_INFO(node->get_logger(), "Updating Wrench Thresholds"); + + wrench_tolerances_.timeout = rclcpp::Duration::from_seconds(force_gate_params_.wrench_threshold.timeout); + wrench_tolerances_.forceTotal = force_gate_params_.wrench_threshold.fMag; + wrench_tolerances_.forceVec[0] = force_gate_params_.wrench_threshold.fx; + wrench_tolerances_.forceVec[1] = force_gate_params_.wrench_threshold.fy; + wrench_tolerances_.forceVec[2] = force_gate_params_.wrench_threshold.fz; + wrench_tolerances_.torqueTotal = force_gate_params_.wrench_threshold.tMag; + wrench_tolerances_.torqueVec[0] = force_gate_params_.wrench_threshold.tx; + wrench_tolerances_.torqueVec[1] = force_gate_params_.wrench_threshold.ty; + wrench_tolerances_.torqueVec[2] = force_gate_params_.wrench_threshold.tz; + + // Subscribe to wrench topic. The topic is read only, so won't be changed. + if (!wrench_subscriber_) { + wrench_subscriber_ = node->create_subscription( + force_gate_params_.wrench_threshold.topic, rclcpp::QoS(1).best_effort().durability_volatile(), + [this](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { + rt_wrench_stamped_.writeFromNonRT(msg); + }); + } + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +bool ForceGateParent::check_wrench_threshold(std::shared_ptr node, const rclcpp::Time & time) +{ + // Check if enabled + if (force_gate_params_.wrench_threshold.topic == "") + { + return true; + } + + // Throttle the warning messages + auto& clock = *node->get_clock(); + int throttle_ms = 1000; + + // Read stamped wrench + auto wrench_stamped = *rt_wrench_stamped_.readFromRT(); + if (!wrench_stamped) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "WrenchStamped not received."); + return false; + } + + // Check all relevant tolerances + if (wrench_tolerances_.timeout != rclcpp::Duration(0, 0)) { + if (time - wrench_tolerances_.timeout > wrench_stamped->header.stamp) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "WrenchStamped timeout."); + return false; + } + } + + double forceSumSq = 0.0; + if (wrench_tolerances_.forceVec[0] != 0.0) { + if (wrench_stamped->wrench.force.x > wrench_tolerances_.forceVec[0]) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "Wrench: Fx violation."); + return false; + } + } + forceSumSq += wrench_stamped->wrench.force.x * wrench_stamped->wrench.force.x; + if (wrench_tolerances_.forceVec[1] != 0.0) { + if (wrench_stamped->wrench.force.y > wrench_tolerances_.forceVec[1]) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "Wrench: Fy violation."); + return false; + } + } + forceSumSq += wrench_stamped->wrench.force.y * wrench_stamped->wrench.force.y; + if (wrench_tolerances_.forceVec[2] != 0.0) { + if (wrench_stamped->wrench.force.z > wrench_tolerances_.forceVec[2]) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "Wrench: Fz violation."); + return false; + } + } + forceSumSq += wrench_stamped->wrench.force.z * wrench_stamped->wrench.force.z; + if (wrench_tolerances_.forceTotal != 0.0) { + if (forceSumSq > wrench_tolerances_.forceTotal*wrench_tolerances_.forceTotal) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "Wrench: ||F|| violation."); + return false; + } + } + + double torqueSumSq = 0.0; + if (wrench_tolerances_.torqueVec[0] != 0.0) { + if (wrench_stamped->wrench.torque.x > wrench_tolerances_.torqueVec[0]) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "Wrench: Tx violation."); + return false; + } + } + torqueSumSq += wrench_stamped->wrench.torque.x * wrench_stamped->wrench.torque.x; + if (wrench_tolerances_.torqueVec[1] != 0.0) { + if (wrench_stamped->wrench.torque.y > wrench_tolerances_.torqueVec[1]) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "Wrench: Ty violation."); + return false; + } + } + torqueSumSq += wrench_stamped->wrench.torque.y * wrench_stamped->wrench.torque.y; + if (wrench_tolerances_.torqueVec[2] != 0.0) { + if (wrench_stamped->wrench.torque.z > wrench_tolerances_.torqueVec[2]) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "Wrench: Tz violation."); + return false; + } + } + torqueSumSq += wrench_stamped->wrench.torque.z * wrench_stamped->wrench.torque.z; + if (wrench_tolerances_.torqueTotal != 0.0) { + if (torqueSumSq > wrench_tolerances_.torqueTotal*wrench_tolerances_.torqueTotal) { + RCLCPP_WARN_THROTTLE(node->get_logger(), clock, throttle_ms, "Wrench: ||T|| violation."); + return false; + } + } + + return true; +} + +} // namespace force_gate_controller diff --git a/force_gate_controller/src/force_gate_position_controller.cpp b/force_gate_controller/src/force_gate_position_controller.cpp new file mode 100644 index 0000000..1500a20 --- /dev/null +++ b/force_gate_controller/src/force_gate_position_controller.cpp @@ -0,0 +1,68 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 "controller_interface/controller_interface.hpp" +#include "force_gate_controller/force_gate_position_controller.hpp" +// #include "hardware_interface/types/hardware_interface_type_values.hpp" +// #include "rclcpp/logging.hpp" +// #include "rclcpp/parameter.hpp" + +namespace force_gate_controller +{ +ForceGatePositionController::ForceGatePositionController() +: position_controllers::JointGroupPositionController() +{ +} + +controller_interface::CallbackReturn ForceGatePositionController::read_parameters() +{ + auto ret = position_controllers::JointGroupPositionController::read_parameters(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + return read_force_gate_parameters(get_node()); +} + +controller_interface::CallbackReturn ForceGatePositionController::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + // When the controller is activated, get the most up-to-date wrench tolerances + auto ret = read_force_gate_parameters(get_node()); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + return position_controllers::JointGroupPositionController::on_activate(previous_state); +} + +controller_interface::return_type ForceGatePositionController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (!check_wrench_threshold(get_node(), get_node()->now())) + { + return controller_interface::return_type::OK; + } + + return position_controllers::JointGroupPositionController::update(time, period); +} + +} // namespace force_gate_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + force_gate_controller::ForceGatePositionController, controller_interface::ControllerInterface) diff --git a/force_gate_controller/src/force_gate_velocity_controller.cpp b/force_gate_controller/src/force_gate_velocity_controller.cpp new file mode 100644 index 0000000..5d30c9f --- /dev/null +++ b/force_gate_controller/src/force_gate_velocity_controller.cpp @@ -0,0 +1,73 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 "controller_interface/controller_interface.hpp" +#include "force_gate_controller/force_gate_velocity_controller.hpp" +// #include "hardware_interface/types/hardware_interface_type_values.hpp" +// #include "rclcpp/logging.hpp" +// #include "rclcpp/parameter.hpp" + +namespace force_gate_controller +{ +ForceGateVelocityController::ForceGateVelocityController() +: velocity_controllers::JointGroupVelocityController() +{ +} + +controller_interface::CallbackReturn ForceGateVelocityController::read_parameters() +{ + auto ret = velocity_controllers::JointGroupVelocityController::read_parameters(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + return read_force_gate_parameters(get_node()); +} + +controller_interface::CallbackReturn ForceGateVelocityController::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + // When the controller is activated, get the most up-to-date wrench tolerances + auto ret = read_force_gate_parameters(get_node()); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + return velocity_controllers::JointGroupVelocityController::on_activate(previous_state); +} + +controller_interface::return_type ForceGateVelocityController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (!check_wrench_threshold(get_node(), get_node()->now())) + { + // stop all joints + for (auto & command_interface : command_interfaces_) + { + command_interface.set_value(0.0); + } + return controller_interface::return_type::OK; + } + + return velocity_controllers::JointGroupVelocityController::update(time, period); +} + +} // namespace force_gate_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + force_gate_controller::ForceGateVelocityController, controller_interface::ControllerInterface) diff --git a/force_gate_controller/src/joint_trajectory_controller_plugin.cpp b/force_gate_controller/src/joint_trajectory_controller_plugin.cpp new file mode 100644 index 0000000..28376c8 --- /dev/null +++ b/force_gate_controller/src/joint_trajectory_controller_plugin.cpp @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Fraunhofer IPA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Fraunhofer IPA nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Mathias Lüdtke */ + +#include +#include +#include +#include +#include + +namespace force_gate_controller +{ +/** + * \brief Simple allocator for moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle instances. + */ +class JointTrajectoryControllerAllocator : public moveit_ros_control_interface::ControllerHandleAllocator +{ +public: + moveit_controller_manager::MoveItControllerHandlePtr alloc(const rclcpp::Node::SharedPtr& node, + const std::string& name, + const std::vector& /* resources */) override + { + return std::make_shared( + node, name, "follow_joint_trajectory"); + } +}; + +} // namespace moveit_ros_control_interface + +PLUGINLIB_EXPORT_CLASS(force_gate_controller::JointTrajectoryControllerAllocator, + moveit_ros_control_interface::ControllerHandleAllocator); diff --git a/force_gate_controller/src/tolerances.cpp b/force_gate_controller/src/tolerances.cpp new file mode 100644 index 0000000..3434a70 --- /dev/null +++ b/force_gate_controller/src/tolerances.cpp @@ -0,0 +1,138 @@ +// Copyright 2013 PAL Robotics S.L. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#include "force_gate_controller/tolerances.hpp" + +namespace force_gate_controller +{ +/** + * \brief Populate trajectory segment tolerances using data from the ROS node. + * + * It is assumed that the following parameter structure is followed on the provided LifecycleNode. + * Unspecified parameters will take the defaults shown in the comments: + * + * \code + * constraints: + * goal_time: 1.0 # Defaults to zero + * stopped_velocity_tolerance: 0.02 # Defaults to 0.01 + * foo_joint: + * trajectory: 0.05 # Defaults to zero (ie. the tolerance is not enforced) + * goal: 0.03 # Defaults to zero (ie. the tolerance is not enforced) + * bar_joint: + * goal: 0.01 + * \endcode + * + * \param params The ROS Parameters + * \return Trajectory segment tolerances. + */ +SegmentTolerances get_segment_tolerances(Params const & params) +{ + auto const & constraints = params.constraints; + auto const n_joints = params.joints.size(); + + SegmentTolerances tolerances; + tolerances.goal_time_tolerance = constraints.goal_time; + + // State and goal state tolerances + tolerances.state_tolerance.resize(n_joints); + tolerances.goal_state_tolerance.resize(n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + auto const joint = params.joints[i]; + tolerances.state_tolerance[i].position = constraints.joints_map.at(joint).trajectory; + tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; + tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; + + auto logger = rclcpp::get_logger("tolerance"); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + } + + return tolerances; +} + +/** + * \param state_error State error to check. + * \param joint_idx Joint index for the state error + * \param state_tolerance State tolerance of joint to check \p state_error against. + * \param show_errors If the joint that violate its tolerance should be output to console. NOT + * REALTIME if true \return True if \p state_error fulfills \p state_tolerance. + */ +bool check_state_tolerance_per_joint( + const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx, + const StateTolerances & state_tolerance, bool show_errors = false) +{ + using std::abs; + const double error_position = state_error.positions[joint_idx]; + const double error_velocity = + state_error.velocities.empty() ? 0.0 : state_error.velocities[joint_idx]; + const double error_acceleration = + state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx]; + + const bool is_valid = + !(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) && + !(state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) && + !(state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration); + + if (is_valid) + { + return true; + } + + if (show_errors) + { + const auto logger = rclcpp::get_logger("tolerances"); + RCLCPP_ERROR(logger, "Path state tolerances failed:"); + + if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) + { + RCLCPP_ERROR( + logger, "Position Error: %f, Position Tolerance: %f", error_position, + state_tolerance.position); + } + if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) + { + RCLCPP_ERROR( + logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity, + state_tolerance.velocity); + } + if ( + state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration) + { + RCLCPP_ERROR( + logger, "Acceleration Error: %f, Acceleration Tolerance: %f", error_acceleration, + state_tolerance.acceleration); + } + } + return false; +} + +} // namespace force_gate_controller diff --git a/force_gate_controller/src/validate_jtc_parameters.cpp b/force_gate_controller/src/validate_jtc_parameters.cpp new file mode 100644 index 0000000..f7c3f68 --- /dev/null +++ b/force_gate_controller/src/validate_jtc_parameters.cpp @@ -0,0 +1,89 @@ +// Copyright (c) 2022 ros2_control Development Team +// +// 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 "force_gate_controller/validate_jtc_parameters.hpp" + +namespace force_gate_controller +{ +tl::expected command_interface_type_combinations( + rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Check if command interfaces combination is valid. Valid combinations are: + // 1. effort + // 2. velocity + // 2. position [velocity, [acceleration]] + + if ( + rsl::contains>(interface_types, "velocity") && + interface_types.size() > 1 && + !rsl::contains>(interface_types, "position")) + { + return tl::make_unexpected( + "'velocity' command interface can be used either alone or 'position' " + "interface has to be present"); + } + + if ( + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position"))) + { + return tl::make_unexpected( + "'acceleration' command interface can only be used if 'velocity' and " + "'position' interfaces are present"); + } + + if ( + rsl::contains>(interface_types, "effort") && + interface_types.size() > 1) + { + return tl::make_unexpected("'effort' command interface has to be used alone"); + } + + return {}; +} + +tl::expected state_interface_type_combinations( + rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Valid combinations are + // 1. position [velocity, [acceleration]] + + if ( + rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position")) + { + return tl::make_unexpected( + "'velocity' state interface cannot be used if 'position' interface " + "is missing."); + } + + if ( + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "position") || + !rsl::contains>(interface_types, "velocity"))) + { + return tl::make_unexpected( + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); + } + + return {}; +} + +} // namespace force_gate_controller