diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 1c57c6e4f3..2685a701e7 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -1,169 +1,204 @@ -cmake_minimum_required(VERSION 3.5) -project(hardware_interface) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) -endif() - -find_package(ament_cmake REQUIRED) -find_package(control_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rcpputils REQUIRED) -find_package(rcutils REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) - -add_library( - hardware_interface - SHARED - src/actuator.cpp - src/component_parser.cpp - src/resource_manager.cpp - src/sensor.cpp - src/system.cpp -) -target_include_directories( - hardware_interface - PUBLIC - include -) -ament_target_dependencies( - hardware_interface - control_msgs - pluginlib - rcutils - rcpputils -) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(hardware_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -# Fake components -add_library( - fake_components - SHARED - src/fake_components/generic_system.cpp -) -target_include_directories( - fake_components - PUBLIC - include -) -target_link_libraries( - fake_components - hardware_interface -) -ament_target_dependencies( - fake_components - pluginlib - rcpputils -) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(fake_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file( - hardware_interface fake_components_plugin_description.xml) - -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - fake_components - hardware_interface - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gmock REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_macros test/test_macros.cpp) - target_include_directories(test_macros PRIVATE include) - ament_target_dependencies(test_macros rcpputils) - - ament_add_gmock(test_joint_handle test/test_handle.cpp) - target_link_libraries(test_joint_handle hardware_interface) - ament_target_dependencies(test_joint_handle rcpputils) - - ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) - target_link_libraries(test_component_interfaces hardware_interface) - - ament_add_gmock(test_component_parser test/test_component_parser.cpp) - target_link_libraries(test_component_parser hardware_interface) - ament_target_dependencies(test_component_parser ros2_control_test_assets) - - add_library(test_components SHARED - test/test_components/test_actuator.cpp - test/test_components/test_sensor.cpp - test/test_components/test_system.cpp) - target_link_libraries(test_components hardware_interface) - ament_target_dependencies(test_components - pluginlib) - install(TARGETS test_components - DESTINATION lib - ) - pluginlib_export_plugin_description_file( - hardware_interface test/test_components/test_components.xml) - - add_library(test_hardware_components SHARED - test/test_hardware_components/test_single_joint_actuator.cpp - test/test_hardware_components/test_force_torque_sensor.cpp - test/test_hardware_components/test_two_joint_system.cpp - test/test_hardware_components/test_system_with_command_modes.cpp - ) - target_link_libraries(test_hardware_components hardware_interface) - ament_target_dependencies(test_hardware_components - pluginlib) - install(TARGETS test_hardware_components - DESTINATION lib - ) - pluginlib_export_plugin_description_file( - hardware_interface test/test_hardware_components/test_hardware_components.xml - ) - - ament_add_gmock(test_resource_manager test/test_resource_manager.cpp) - target_link_libraries(test_resource_manager hardware_interface) - ament_target_dependencies(test_resource_manager ros2_control_test_assets) - - ament_add_gmock(test_generic_system test/fake_components/test_generic_system.cpp) - target_include_directories(test_generic_system PRIVATE include) - target_link_libraries(test_generic_system hardware_interface) - ament_target_dependencies(test_generic_system - pluginlib - ros2_control_test_assets - ) -endif() - -ament_export_include_directories( - include -) -ament_export_libraries( - fake_components - hardware_interface -) -ament_export_dependencies( - control_msgs - pluginlib - rcpputils - tinyxml2_vendor - TinyXML2 -) -ament_package() +cmake_minimum_required(VERSION 3.5) +project(hardware_interface) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rcutils REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) +find_package(urdf REQUIRED) + +add_library( + hardware_interface + SHARED + src/actuator.cpp + src/component_parser.cpp + src/resource_manager.cpp + src/sensor.cpp + src/system.cpp +) +target_include_directories( + hardware_interface + PUBLIC + include +) +ament_target_dependencies( + hardware_interface + control_msgs + pluginlib + rcutils + rcpputils + urdf +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +# prevent pluginlib from using boost +target_compile_definitions(hardware_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +# Fake components +add_library( + fake_components + SHARED + src/fake_components/generic_system.cpp +) +target_include_directories( + fake_components + PUBLIC + include +) +target_link_libraries( + fake_components + hardware_interface +) +ament_target_dependencies( + fake_components + pluginlib + rcpputils +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +# prevent pluginlib from using boost +target_compile_definitions(fake_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file( + hardware_interface fake_components_plugin_description.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + fake_components + hardware_interface + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(rclcpp REQUIRED) + + ament_add_gmock(test_macros test/test_macros.cpp) + target_include_directories(test_macros PRIVATE include) + ament_target_dependencies(test_macros rcpputils) + + ament_add_gmock(test_joint_handle test/test_handle.cpp) + target_link_libraries(test_joint_handle hardware_interface) + ament_target_dependencies(test_joint_handle rcpputils) + + ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) + target_link_libraries(test_component_interfaces hardware_interface) + + ament_add_gmock(test_component_parser test/test_component_parser.cpp) + target_link_libraries(test_component_parser hardware_interface) + ament_target_dependencies(test_component_parser ros2_control_test_assets) + + add_library(test_components SHARED + test/test_components/test_actuator.cpp + test/test_components/test_sensor.cpp + test/test_components/test_system.cpp) + target_link_libraries(test_components hardware_interface) + ament_target_dependencies(test_components + pluginlib) + install(TARGETS test_components + DESTINATION lib + ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_components/test_components.xml) + + add_library(test_hardware_components SHARED + test/test_hardware_components/test_single_joint_actuator.cpp + test/test_hardware_components/test_force_torque_sensor.cpp + test/test_hardware_components/test_two_joint_system.cpp + ) + target_link_libraries(test_hardware_components hardware_interface) + ament_target_dependencies(test_hardware_components + pluginlib) + install(TARGETS test_hardware_components + DESTINATION lib + ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_hardware_components/test_hardware_components.xml + ) + + ament_add_gmock(test_resource_manager test/test_resource_manager.cpp) + target_link_libraries(test_resource_manager hardware_interface) + ament_target_dependencies(test_resource_manager ros2_control_test_assets) + + ament_add_gmock(test_generic_system test/fake_components/test_generic_system.cpp) + target_include_directories(test_generic_system PRIVATE include) + target_link_libraries(test_generic_system hardware_interface) + ament_target_dependencies(test_generic_system + pluginlib + ros2_control_test_assets + ) + + # joint_limiter tests + ament_add_gmock(test_joint_limiter_interface test/joint_limits/test_joint_limiter_interface.cpp) + target_include_directories(test_joint_limiter_interface PUBLIC include) + ament_target_dependencies(test_joint_limiter_interface rcutils) + + # joint_limit_interface tests + ament_add_gmock(joint_limits_interface_test test/joint_limits/joint_limits_interface_test.cpp) + target_include_directories(joint_limits_interface_test PUBLIC include) + ament_target_dependencies(joint_limits_interface_test rclcpp) + + add_executable(joint_limits_rosparam_test test/joint_limits/joint_limits_rosparam_test.cpp) + target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) + target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) + ament_target_dependencies(joint_limits_rosparam_test rclcpp) + add_launch_test(test/joint_limits/joint_limits_rosparam.launch.py) + install( + TARGETS + joint_limits_rosparam_test + DESTINATION lib/${PROJECT_NAME} + ) + install( + FILES + test/joint_limits/joint_limits_rosparam.yaml + DESTINATION share/${PROJECT_NAME}/test + ) + + ament_add_gmock(joint_limits_urdf_test test/joint_limits/joint_limits_urdf_test.cpp) + target_include_directories(joint_limits_urdf_test PUBLIC include) + ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + fake_components + hardware_interface +) +ament_export_dependencies( + control_msgs + pluginlib + rcpputils + tinyxml2_vendor + TinyXML2 + urdf +) +ament_package() diff --git a/joint_limits_interface/README.md b/hardware_interface/JointLimits-README.md similarity index 100% rename from joint_limits_interface/README.md rename to hardware_interface/JointLimits-README.md diff --git a/hardware_interface/doc/joint_limits_interface/JointLimitInterface.drawio b/hardware_interface/doc/joint_limits_interface/JointLimitInterface.drawio new file mode 100644 index 0000000000..cec461741c --- /dev/null +++ b/hardware_interface/doc/joint_limits_interface/JointLimitInterface.drawio @@ -0,0 +1 @@ +7Zrdc9o4EMD/GmbSh+tgGRt4LCRN7655KXPXjxdGsYWtxrZ8sghwf/2tZMkf2FAHCrRXZjLBWn0g7e5Pu5Lp2dN4fc9xGj4wn0Q91PfXPfu2hxDquwg+pGSTSyxrZOeSgFNfy0rBjP5LtLCvpUvqk6zWUDAWCZrWhR5LEuKJmgxzzlb1ZgsW1b81xQFpCGYejprSj9QXYS4doWEpf0doEJpvttxxXhNj01ivJAuxz1YVkX3Xs6ecMZE/xespiaT2jF7yfm931BYT4yQRXTrc3yPi/EnIl+xv92ltWffuw/w3PcozjpZ6wR9IxpbcIzPBuNLNG/ijCRUUR2Cd+Q32xBJDZW84zUiS5Q+bTJD4lV6o2BjtcbZMfCIn0O/Zk1VIBZml2JO1K3AYkIUijqBkweOCRtGURTCgfZuwBBpN4CuDBIo81/DkmXBBwThvtFwwOcaCJUJ7juWasp6EWSB0JOudmrMKe4AnExYTwTfQxLixq02onXigi6vSIQa2loUVZ3CHWoi1EwbF0KWd4EGb6gVmcxuaJj64rS4yLkIWsARHd6V0UrdF2eY9k0pUFvhKhNhoTeKlYHX7kDUVn2T3144ufa7U3K71yKqwMYUEllvpJIufq3VlN1Uy/aomBctM8vXKRe63IehEee8e3ekdSWAeELGnndPuE5xEWNDn+jy+u4FRg8sKgiHm/gpWfnMscFtK3gOgB2omvDDEUTgNrDpOltPkyRq08GSfCqfBFaeDcbI74oQuiZPdwOndx3y9GZgQ9f/6cPtWZikk8zhNBWVJT0ZTF8cSmfx/0WzLVQAEUbdsJjh7IlskdYFrO7zF1PeVs7WBXHfAdgsfxymqczpuwRSdE9PRFdODMXU6Yjq8JKZOM+rFKdhsngksCOSZHotjnPhzKoFZAA7ZTxoD7VGdreK8U4EL9c8Jl5nAla4D6Bp2pGt8SbqGDboy4efnu2c4OoPLm4AXCRXFdCWEM5oERWUg8mComKyQqKSu6voohwpqgxzyDU3aC6t91/gbkUXb4fKy0dd2nPoOMWzZIZyz7hDoukMcvEOMO+4Q1g6vOM8WMW5sESpFBtFXBhTCZ0RjKjJ5k8ZZXGbOgKkENiTek3TRkHqhuoAjKqte0ESm2f+DOG2jjnF6cDIKnSuFB1NomSvkb2I4uCSGZpoVDiOGJWF/SAzfSwbBwwG4CGcKRhla+wQr6jSq26H45hErksUmVbe69cF+NzH2VbNnUwJI46WUyWFmkKRHpD6znxH14RbqLedd1HbePdktL7qm5EegPuiK+kWTcjPNrlm5gnuuo7Bq1Urxdi6tS8ljpq+yKuMQ/kvl1Vu3WoMWzG3knDOijxou0DBGAHpJd65ev/zEj6Z5/6VaKVantTJs0UrrOy7rdMcNt6EW827yASc4kPu+9P8VBy86+hrocu8e9/hE01wXNEfz+gCUIbvB7vLPUr7Fhs0dEhGPmO2pkBcZitp1ZMLCFmW+Asf8FFSeiB/2Lu9FJlrXswINlNuyy5z13NCyqVyTic7JRNfj+65t9kzJRPP8nkEGQcy5XUZ6GEK+3MI9+ROVVHmkkBSal8pVIOE5wTHJp4r6T2TzgwL6rcweva5fpjktOLbm9s7JcGwe8Yq8T9mlmvTtuVc1V6dTZZaTJY7d0sh5jNNfOJV0W34Y1JpKHuBVUCx/KqbqKr+4s+/+Aw== \ No newline at end of file diff --git a/hardware_interface/doc/joint_limits_interface/JointLimitInterface.png b/hardware_interface/doc/joint_limits_interface/JointLimitInterface.png new file mode 100644 index 0000000000..f58f7c7e7d Binary files /dev/null and b/hardware_interface/doc/joint_limits_interface/JointLimitInterface.png differ diff --git a/hardware_interface/include/joint_limits/joint_limiter_interface.hpp b/hardware_interface/include/joint_limits/joint_limiter_interface.hpp new file mode 100644 index 0000000000..931e12dc64 --- /dev/null +++ b/hardware_interface/include/joint_limits/joint_limiter_interface.hpp @@ -0,0 +1,170 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace joint_limits +{ +// TODO(all): there is possibility to use something like LoanedCommandInterface. +// Still, having copy-constructor on this level makes thing way easier and more readable +// class LimiterCommandHandle + +template +class JointLimiterInterface +{ +public: + JointLimiterInterface( + LimitsType & limits, + std::vector & state_interfaces, + // Here could be used "LimiterCommandHandle" with copy-constructor + // to avoid raw-pointer access/storage + std::vector & command_interfaces) + : limits_(limits), + has_effort_command_(false), + has_velocity_state_(false), + previous_position_(std::numeric_limits::quiet_NaN()), + previous_velocity_(0.0) + { + // At least one command interface has to be provided + if (command_interfaces.size() == 0) { + throw std::runtime_error("At least one command interface has to be provided."); + } + + // Check if position-velocity-acceleration or effort commands are used + if ( + std::find_if( + command_interfaces.begin(), command_interfaces.end(), + [](const hardware_interface::CommandInterface * interface) { + return interface->get_name() == hardware_interface::HW_IF_EFFORT; + }) != command_interfaces.end()) + { + has_effort_command_ = true; + // Effort command interface has to be used as only interface + if (command_interfaces.size() > 1) { + // TODO(all): Do we need here specific exception type? + throw std::runtime_error("Effort command interface has to be used alone."); + } + } + + for (const auto & interface_name : interface_order_) { + for (const auto & interface : command_interfaces) { + if (interface_name == interface->get_name()) { + command_interfaces_.emplace_back(interface); + } else { + // Do not create 'effort' virtual interfaces if effort command interface is used + if (!has_effort_command_ || interface_name != hardware_interface::HW_IF_EFFORT) { + virtual_command_storage_.push_back(0.0); + auto i = virtual_command_storage_.size() - 1; // last element + virtual_command_interfaces_.emplace_back( + hardware_interface::CommandInterface( + "joint_limiter_virtual", interface_name, &virtual_command_storage_[i])); + command_interfaces_.emplace_back(&virtual_command_interfaces_.back()); + } + } + } + for (const auto & interface : state_interfaces) { + if (interface_name == interface.get_name()) { + state_interfaces_.emplace_back(interface); + } else { + virtual_state_storage_.push_back(0.0); + auto i = virtual_state_storage_.size() - 1; // last element + state_interfaces_.emplace_back( + hardware_interface::StateInterface( + "joint_limiter_virtual", interface_name, &virtual_state_storage_[i])); + } + } + } + } + + ~JointLimiterInterface() = default; + + /// Implementation of limit enforcing policy. + virtual void enforce_limits(const rclcpp::Duration & period) + { + if (std::isnan(previous_position_)) { + previous_position_ = state_interfaces_[0].get_value(); + } + + if (has_effort_command_) { + enforce_effort_limits(period); + } else { + enforce_pos_vel_acc_limits(period); + } + } + + std::string get_name() const + { + // At least one command interface exists - return its name + for (const auto & interface : command_interfaces_) { + return interface->get_name(); + } + return std::string(); + } + + /// Clear stored state, causing to reset next iteration. + virtual void reset() + { + previous_position_ = std::numeric_limits::quiet_NaN(); + previous_velocity_ = 0.0; + } + +protected: + /// Limit enforcing policy for effort command interface. + virtual void enforce_effort_limits(const rclcpp::Duration & period) = 0; + + /// Limit enforcing policy for position, velocity and acceleration command interfaces. + virtual void enforce_pos_vel_acc_limits(const rclcpp::Duration & period) = 0; + + LimitsType & limits_; + std::vector command_interfaces_; + std::vector state_interfaces_; + std::vector virtual_command_interfaces_; + std::vector virtual_state_interfaces_; + std::vector virtual_command_storage_; + std::vector virtual_state_storage_; + + bool has_effort_command_; + bool has_velocity_state_; + + // stored states + double previous_position_; + double previous_velocity_; + + // min and max values + double min_position_limit_, max_position_limit_; + double max_velocity_limit_; + double max_acceleration_limit_; + + std::vector interface_order_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/hardware_interface/include/joint_limits/joint_limits.hpp b/hardware_interface/include/joint_limits/joint_limits.hpp new file mode 100644 index 0000000000..e3c0e1c0c2 --- /dev/null +++ b/hardware_interface/include/joint_limits/joint_limits.hpp @@ -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. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_HPP_ + +namespace joint_limits +{ +struct JointLimits +{ + JointLimits() + : min_position(0.0), + max_position(0.0), + max_velocity(0.0), + max_acceleration(0.0), + max_jerk(0.0), + max_effort(0.0), + has_position_limits(false), + has_velocity_limits(false), + has_acceleration_limits(false), + has_jerk_limits(false), + has_effort_limits(false), + angle_wraparound(false) + { + } + + double min_position; + double max_position; + double max_velocity; + double max_acceleration; + double max_jerk; + double max_effort; + + bool has_position_limits; + bool has_velocity_limits; + bool has_acceleration_limits; + bool has_jerk_limits; + bool has_effort_limits; + bool angle_wraparound; +}; + +struct SoftJointLimits +{ + SoftJointLimits() + : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0) {} + + double min_position; + double max_position; + double k_position; + double k_velocity; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_HPP_ diff --git a/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp b/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp new file mode 100644 index 0000000000..7d36bb3843 --- /dev/null +++ b/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp @@ -0,0 +1,249 @@ +// 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. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ + +#include + +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace joint_limits +{ +/// Populate a JointLimits instance from the ROS parameter server. +/** + * It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters + * are simply not added to the joint limits specification. + * \code + * joint_limits: + * foo_joint: + * has_position_limits: true + * min_position: 0.0 + * max_position: 1.0 + * has_velocity_limits: true + * max_velocity: 2.0 + * has_acceleration_limits: true + * max_acceleration: 5.0 + * has_jerk_limits: true + * max_jerk: 100.0 + * has_effort_limits: true + * max_effort: 20.0 + * bar_joint: + * has_position_limits: false # Continuous joint + * has_velocity_limits: true + * max_velocity: 4.0 + * \endcode + * + * This specification is similar to the one used by MoveIt!, + * but additionally supports jerk and effort limits. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node NodeHandle where the joint limits are specified. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), false otherwise. + */ +inline bool getJointLimits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try { + if ( + !node->has_parameter(param_base_name + ".has_position_limits") && + !node->has_parameter(param_base_name + ".min_position") && + !node->has_parameter(param_base_name + ".max_position") && + !node->has_parameter(param_base_name + ".has_velocity_limits") && + !node->has_parameter(param_base_name + ".min_velocity") && + !node->has_parameter(param_base_name + ".max_velocity") && + !node->has_parameter(param_base_name + ".has_acceleration_limits") && + !node->has_parameter(param_base_name + ".max_acceleration") && + !node->has_parameter(param_base_name + ".has_jerk_limits") && + !node->has_parameter(param_base_name + ".max_jerk") && + !node->has_parameter(param_base_name + ".has_effort_limits") && + !node->has_parameter(param_base_name + ".max_effort") && + !node->has_parameter(param_base_name + ".angle_wraparound") && + !node->has_parameter(param_base_name + ".has_soft_limits") && + !node->has_parameter(param_base_name + ".k_position") && + !node->has_parameter(param_base_name + ".k_velocity") && + !node->has_parameter(param_base_name + ".soft_lower_limit") && + !node->has_parameter(param_base_name + ".soft_upper_limit")) + { + RCLCPP_ERROR_STREAM( + node->get_logger(), "No joint limits specification found for joint '" << + joint_name << "' in the parameter server (node: " << + std::string(node->get_name()) + " param name: " + param_base_name << + ")."); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR_STREAM(node->get_logger(), ex.what()); + return false; + } + + // Position limits + bool has_position_limits = false; + if (node->get_parameter(param_base_name + ".has_position_limits", has_position_limits)) { + if (!has_position_limits) { + limits.has_position_limits = false; + } + double min_pos, max_pos; + if ( + has_position_limits && node->get_parameter(param_base_name + ".min_position", min_pos) && + node->get_parameter(param_base_name + ".max_position", max_pos)) + { + limits.has_position_limits = true; + limits.min_position = min_pos; + limits.max_position = max_pos; + } + + bool angle_wraparound; + if ( + !has_position_limits && + node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound)) + { + limits.angle_wraparound = angle_wraparound; + } + } + + // Velocity limits + bool has_velocity_limits = false; + if (node->get_parameter(param_base_name + ".has_velocity_limits", has_velocity_limits)) { + if (!has_velocity_limits) { + limits.has_velocity_limits = false; + } + double max_vel; + if (has_velocity_limits && node->get_parameter(param_base_name + ".max_velocity", max_vel)) { + limits.has_velocity_limits = true; + limits.max_velocity = max_vel; + } + } + + // Acceleration limits + bool has_acceleration_limits = false; + if (node->get_parameter(param_base_name + ".has_acceleration_limits", has_acceleration_limits)) { + if (!has_acceleration_limits) { + limits.has_acceleration_limits = false; + } + double max_acc; + if ( + has_acceleration_limits && + node->get_parameter(param_base_name + ".max_acceleration", max_acc)) + { + limits.has_acceleration_limits = true; + limits.max_acceleration = max_acc; + } + } + + // Jerk limits + bool has_jerk_limits = false; + if (node->get_parameter(param_base_name + ".has_jerk_limits", has_jerk_limits)) { + if (!has_jerk_limits) { + limits.has_jerk_limits = false; + } + double max_jerk; + if (has_jerk_limits && node->get_parameter(param_base_name + ".max_jerk", max_jerk)) { + limits.has_jerk_limits = true; + limits.max_jerk = max_jerk; + } + } + + // Effort limits + bool has_effort_limits = false; + if (node->get_parameter(param_base_name + ".has_effort_limits", has_effort_limits)) { + if (!has_effort_limits) { + limits.has_effort_limits = false; + } + double max_effort; + if (has_effort_limits && node->get_parameter(param_base_name + ".max_effort", max_effort)) { + limits.has_effort_limits = true; + limits.max_effort = max_effort; + } + } + + return true; +} + +/// Populate a SoftJointLimits instance from the ROS parameter server. +/** + * It is assumed that the following parameter structure is followed on the provided NodeHandle. Only completely specified soft + * joint limits specifications will be considered valid. + * \code + * joint_limits: + * foo_joint: + * soft_lower_limit: 0.0 + * soft_upper_limit: 1.0 + * k_position: 10.0 + * k_velocity: 10.0 + * \endcode + * + * This specification is similar to the specification of the safety_controller tag in the URDF, adapted to the parameter server. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node NodeHandle where the joint limits are specified. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. + * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and + * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. + */ +inline bool getSoftJointLimits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, + SoftJointLimits & soft_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try { + if ( + !node->has_parameter(param_base_name + ".has_soft_limits") && + !node->has_parameter(param_base_name + ".k_velocity") && + !node->has_parameter(param_base_name + ".k_position") && + !node->has_parameter(param_base_name + ".soft_lower_limit") && + !node->has_parameter(param_base_name + ".soft_upper_limit")) + { + RCLCPP_DEBUG_STREAM( + node->get_logger(), "No soft joint limits specification found for joint '" << + joint_name << "' in the parameter server (node: " << + std::string(node->get_name()) + " param name: " + param_base_name << + ")."); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR_STREAM(node->get_logger(), ex.what()); + return false; + } + + // Override soft limits if complete specification is found + bool has_soft_limits; + if (node->get_parameter(param_base_name + ".has_soft_limits", has_soft_limits)) { + if ( + has_soft_limits && node->has_parameter(param_base_name + ".k_position") && + node->has_parameter(param_base_name + ".k_velocity") && + node->has_parameter(param_base_name + ".soft_lower_limit") && + node->has_parameter(param_base_name + ".soft_upper_limit")) + { + node->get_parameter(param_base_name + ".k_position", soft_limits.k_position); + node->get_parameter(param_base_name + ".k_velocity", soft_limits.k_velocity); + node->get_parameter(param_base_name + ".soft_lower_limit", soft_limits.min_position); + node->get_parameter(param_base_name + ".soft_upper_limit", soft_limits.max_position); + return true; + } + } + + return false; +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/hardware_interface/include/joint_limits/joint_limits_urdf.hpp b/hardware_interface/include/joint_limits/joint_limits_urdf.hpp new file mode 100644 index 0000000000..a7f27543ad --- /dev/null +++ b/hardware_interface/include/joint_limits/joint_limits_urdf.hpp @@ -0,0 +1,84 @@ +// 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. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ + +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/rclcpp.hpp" +#include "urdf/urdfdom_compatibility.h" +#include "urdf_model/joint.h" + +namespace joint_limits +{ +/** + * Populate a JointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will overwrite existing + * values. Values in \e limits not present in \e urdf_joint remain unchanged. + * \return True if \e urdf_joint has a valid limits specification, false otherwise. + */ +inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) +{ + if (!urdf_joint || !urdf_joint->limits) { + return false; + } + + limits.has_position_limits = + urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; + if (limits.has_position_limits) { + limits.min_position = urdf_joint->limits->lower; + limits.max_position = urdf_joint->limits->upper; + } + + if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) { + limits.angle_wraparound = true; + } + + limits.has_velocity_limits = true; + limits.max_velocity = urdf_joint->limits->velocity; + + limits.has_acceleration_limits = false; + + limits.has_effort_limits = true; + limits.max_effort = urdf_joint->limits->effort; + + return true; +} + +/** + * Populate a SoftJointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] soft_limits Where URDF soft joint limit data gets written into. + * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. + */ +inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) +{ + if (!urdf_joint || !urdf_joint->safety) { + return false; + } + + soft_limits.min_position = urdf_joint->safety->soft_lower_limit; + soft_limits.max_position = urdf_joint->safety->soft_upper_limit; + soft_limits.k_position = urdf_joint->safety->k_position; + soft_limits.k_velocity = urdf_joint->safety->k_velocity; + + return true; +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ diff --git a/hardware_interface/include/joint_limits/simple_joint_limiter.hpp b/hardware_interface/include/joint_limits/simple_joint_limiter.hpp new file mode 100644 index 0000000000..c7f0f01180 --- /dev/null +++ b/hardware_interface/include/joint_limits/simple_joint_limiter.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ +#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ + +#include + +#include "hardware_interface/handle.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/duration.hpp" + +namespace joint_limits +{ +/// Implementation of simple joint limiter enforcing position, velocity, acceleration and effort +/// limits without considering soft limits. +/** + * This class implements a very simple position, velocity, acceleration and effort limits enforcing + * policy, and tries to do this in hardware-agnostic way. + * This enable its general use in the Resource Manager. + */ +class SimpleJointLimiter : public JointLimiterInterface +{ +public: + SimpleJointLimiter( + JointLimits & limits, std::vector & state_interfaces, + std::vector & command_interfaces); + +protected: + void enforce_effort_limits(const rclcpp::Duration & period) override; + void enforce_pos_vel_acc_limits(const rclcpp::Duration & period) override; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp b/hardware_interface/include/joint_limits_interface/joint_limits.hpp similarity index 100% rename from joint_limits_interface/include/joint_limits_interface/joint_limits.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits.hpp diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp similarity index 73% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp index c2b850d04e..33f7d05284 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/hardware_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -1,715 +1,734 @@ -// 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. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include "joint_limits_interface/joint_limits.hpp" -#include "joint_limits_interface/joint_limits_interface_exception.hpp" - - -namespace joint_limits_interface -{ - -/** - * The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint. - */ -class JointLimitHandle -{ -public: - JointLimitHandle() - : prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0), - jposh_(hardware_interface::HW_IF_POSITION), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_("position_command") - {} - - JointLimitHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : jposh_(jposh), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_(jcmdh), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - {} - - JointLimitHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : jposh_(jposh), - jvelh_(jvelh), - jcmdh_(jcmdh), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - {} - - /// \return Joint name. - std::string get_name() const - { - return jposh_ ? jposh_.get_name() : - jvelh_ ? jvelh_.get_name() : - jcmdh_ ? jcmdh_.get_name() : - std::string(); - } - - /// Sub-class implementation of limit enforcing policy. - virtual void enforce_limits(const rclcpp::Duration & period) = 0; - - /// Clear stored state, causing it to reset next iteration. - virtual void reset() - { - prev_pos_ = std::numeric_limits::quiet_NaN(); - prev_vel_ = 0.0; - } - -protected: - hardware_interface::JointHandle jposh_; - hardware_interface::JointHandle jvelh_; - hardware_interface::JointHandle jcmdh_; - joint_limits_interface::JointLimits limits_; - - // stored state - track position and velocity of last update - double prev_pos_; - double prev_vel_; - - /// Return velocity for limit calculations. - /** - * @param period Time since last measurement - * @return the velocity, from state if available, otherwise from previous position history. - */ - double get_velocity(const rclcpp::Duration & period) const - { - // if we have a handle to a velocity state we can directly return state velocity - // otherwise we will estimate velocity from previous position (command or state) - return jvelh_ ? - jvelh_.get_value() : - (jposh_.get_value() - prev_pos_) / period.seconds(); - } -}; - - -/** The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint that has soft-limits. - */ -class JointSoftLimitsHandle : public JointLimitHandle -{ -public: - JointSoftLimitsHandle() {} - - JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jcmdh, limits), - soft_limits_(soft_limits) - {} - - JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits), - soft_limits_(soft_limits) - {} - -protected: - joint_limits_interface::SoftJointLimits soft_limits_; -}; - - -/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have - soft limits. */ -class PositionJointSaturationHandle : public JointLimitHandle -{ -public: - PositionJointSaturationHandle() {} - - PositionJointSaturationHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : JointLimitHandle(jposh, jcmdh, limits) - { - if (limits_.has_position_limits) { - min_pos_limit_ = limits_.min_position; - max_pos_limit_ = limits_.max_position; - } else { - min_pos_limit_ = -std::numeric_limits::max(); - max_pos_limit_ = std::numeric_limits::max(); - } - } - -/// Enforce position and velocity limits for a joint that is not subject to soft limits. -/** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - if (std::isnan(prev_pos_)) { - prev_pos_ = jposh_.get_value(); - } - - double min_pos, max_pos; - if (limits_.has_velocity_limits) { - // enforce velocity limits - // set constraints on where the position can be based on the - // max velocity times seconds since last update - const double delta_pos = limits_.max_velocity * period.seconds(); - min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); - max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); - } else { - // no velocity limit, so position is simply limited to set extents (our imposed soft limits) - min_pos = min_pos_limit_; - max_pos = max_pos_limit_; - } - - // clamp command position to our computed min/max position - const double cmd = rcppmath::clamp(jcmdh_.get_value(), min_pos, max_pos); - jcmdh_.set_value(cmd); - - prev_pos_ = cmd; - } - -private: - double min_pos_limit_, max_pos_limit_; -}; - -/// A handle used to enforce position and velocity limits of a position-controlled joint. -/** - * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least - * amount of requisites on the underlying hardware platform. - * This lowers considerably the entry barrier to use it, but also implies some limitations. - * - * Requisites - * - Position (for non-continuous joints) and velocity limits specification. - * - Soft limits specification. The \c k_velocity parameter is \e not used. - * - * Open loop nature - * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for - * validity without relying on the actual position/velocity values. - * - * - Actual position values are \e not used because in some platforms there might be a substantial lag - * between sending a command and executing it (propagate command to hardware, reach control objective, - * read from hardware). - * - * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose - * trustworthy velocity measurements, or none at all. - * - * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large - * position tracking errors. Only the command is guaranteed to comply with the limits specification. - * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command - * velocity. - */ - -// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? -class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - PositionJointSoftLimitsHandle() - {} - - PositionJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jcmdh, limits, soft_limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce position and velocity limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity limits will be - * enforced. - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - assert(period.seconds() > 0.0); - - // Current position - if (std::isnan(prev_pos_)) { - // Happens only once at initialization - prev_pos_ = jposh_.get_value(); - } - const double pos = prev_pos_; - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -limits_.max_velocity, - limits_.max_velocity); - } else { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Position bounds - const double dt = period.seconds(); - double pos_low = pos + soft_min_vel * dt; - double pos_high = pos + soft_max_vel * dt; - - if (limits_.has_position_limits) { - // This extra measure safeguards against pathological cases, like when the soft limit lies - // beyond the hard limit - pos_low = std::max(pos_low, limits_.min_position); - pos_high = std::min(pos_high, limits_.max_position); - } - - // Saturate position command according to bounds - const double pos_cmd = rcppmath::clamp( - jcmdh_.get_value(), - pos_low, - pos_high); - jcmdh_.set_value(pos_cmd); - - // Cache variables - // todo: shouldn't this just be pos_cmd? why call into the command handle to - // get what we have in the above line? - prev_pos_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and effort limits of an effort-controlled - * joint that does not have soft limits. - */ -class EffortJointSaturationHandle : public JointLimitHandle -{ -public: - EffortJointSaturationHandle() {} - - EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no efforts limits specification."); - } - } - - EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : EffortJointSaturationHandle(jposh, - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) - { - } - - /** - * Enforce position, velocity, and effort limits for a joint that is not subject - * to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - double min_eff = -limits_.max_effort; - double max_eff = limits_.max_effort; - - if (limits_.has_position_limits) { - const double pos = jposh_.get_value(); - if (pos < limits_.min_position) { - min_eff = 0.0; - } else if (pos > limits_.max_position) { - max_eff = 0.0; - } - } - - const double vel = get_velocity(period); - if (vel < -limits_.max_velocity) { - min_eff = 0.0; - } else if (vel > limits_.max_velocity) { - max_eff = 0.0; - } - - double clamped = rcppmath::clamp(jcmdh_.get_value(), min_eff, max_eff); - jcmdh_.set_value(clamped); - } -}; - -/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. -// TODO(anyone): This class is untested!. Update unit tests accordingly. -class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - EffortJointSoftLimitsHandle() {} - - EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no effort limits specification."); - } - } - - EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : EffortJointSoftLimitsHandle(jposh, - hardware_interface::JointHandle( - hardware_interface::HW_IF_VELOCITY), jcmdh, limits, soft_limits) - { - } - - /// Enforce position, velocity and effort limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits - * will be enforced. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Current state - const double pos = jposh_.get_value(); - const double vel = get_velocity(period); - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -limits_.max_velocity, - limits_.max_velocity); - } else { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Effort bounds depend on the velocity and effort bounds - const double soft_min_eff = rcppmath::clamp( - -soft_limits_.k_velocity * (vel - soft_min_vel), - -limits_.max_effort, - limits_.max_effort); - - const double soft_max_eff = rcppmath::clamp( - -soft_limits_.k_velocity * (vel - soft_max_vel), - -limits_.max_effort, - limits_.max_effort); - - // Saturate effort command according to bounds - const double eff_cmd = rcppmath::clamp( - jcmdh_.get_value(), - soft_min_eff, - soft_max_eff); - jcmdh_.set_value(eff_cmd); - } -}; - - -/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. -class VelocityJointSaturationHandle : public JointLimitHandle -{ -public: - VelocityJointSaturationHandle() {} - - VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jvelh, // currently unused - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(hardware_interface::JointHandle( - hardware_interface::HW_IF_POSITION), jvelh, jcmdh, limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) - { - if (!limits.has_velocity_limits) { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce joint velocity and acceleration limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Velocity bounds - double vel_low; - double vel_high; - - if (limits_.has_acceleration_limits) { - assert(period.seconds() > 0.0); - const double dt = period.seconds(); - - vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); - vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); - } else { - vel_low = -limits_.max_velocity; - vel_high = limits_.max_velocity; - } - - // Saturate velocity command according to limits - const double vel_cmd = rcppmath::clamp( - jcmdh_.get_value(), - vel_low, - vel_high); - jcmdh_.set_value(vel_cmd); - - // Cache variables - prev_vel_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ -class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - VelocityJointSoftLimitsHandle() {} - - VelocityJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, - const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) - { - if (limits.has_velocity_limits) { - max_vel_limit_ = limits.max_velocity; - } else { - max_vel_limit_ = std::numeric_limits::max(); - } - } - - /** - * Enforce position, velocity, and acceleration limits for a velocity-controlled joint - * subject to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - double min_vel, max_vel; - if (limits_.has_position_limits) { - // Velocity bounds depend on the velocity limit and the proximity to the position limit. - const double pos = jposh_.get_value(); - min_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), - -max_vel_limit_, max_vel_limit_); - max_vel = rcppmath::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), - -max_vel_limit_, max_vel_limit_); - } else { - min_vel = -max_vel_limit_; - max_vel = max_vel_limit_; - } - - if (limits_.has_acceleration_limits) { - const double vel = get_velocity(period); - const double delta_t = period.seconds(); - min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); - max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); - } - - jcmdh_.set_value(rcppmath::clamp(jcmdh_.get_value(), min_vel, max_vel)); - } - -private: - double max_vel_limit_; -}; - -// TODO(anyone): Port this to ROS 2 -// //** -// * Interface for enforcing joint limits. -// * -// * \tparam HandleType %Handle type. Must implement the following methods: -// * \code -// * void enforce_limits(); -// * std::string get_name() const; -// * \endcode -// */ -// template -// class joint_limits_interface::JointLimitsInterface -// : public hardware_interface::ResourceManager -// { -// public: -// HandleType getHandle(const std::string & name) -// { -// // Rethrow exception with a meaningful type -// try { -// return this->hardware_interface::ResourceManager::getHandle(name); -// } catch (const std::logic_error & e) { -// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); -// } -// } -// -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Enforce limits for all managed handles. */ -// void enforce_limits(const rclcpp::Duration & period) -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.enforce_limits(period); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint through saturation. */ -// class PositionJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ -// class PositionJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ -// class EffortJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ -// class EffortJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ -// class VelocityJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ -// class VelocityJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ +// 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. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ +#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "joint_limits_interface/joint_limits.hpp" +#include "joint_limits_interface/joint_limits_interface_exception.hpp" + + +namespace joint_limits_interface +{ + +/** + * The base class of limit handles for enforcing position, velocity, and effort limits of + * an effort-controlled joint. + */ +class JointLimitHandle +{ +public: +// JointLimitHandle() +// : joint_position_interface_(hardware_interface::HW_IF_POSITION), +// joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), +// joint_command_interface_(hardware_interface::CommandInterface( +// hardware_interface::HW_IF_POSITION)), +// prev_pos_(std::numeric_limits::quiet_NaN()), +// prev_vel_(std::numeric_limits::quiet_NaN()) +// {} + + JointLimitHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits) + : joint_position_interface_(joint_position_interface), + joint_velocity_interface_(hardware_interface::HW_IF_VELOCITY), + joint_command_interface_(joint_command_interface), + limits_(limits), + prev_pos_(std::numeric_limits::quiet_NaN()), + prev_vel_(0.0) + {} + + JointLimitHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits) + : joint_position_interface_(joint_position_interface), + joint_velocity_interface_(joint_velocity_interface), + joint_command_interface_(joint_command_interface), + limits_(limits), + prev_pos_(std::numeric_limits::quiet_NaN()), + prev_vel_(0.0) + {} + + /// \return Joint name. + std::string get_name() const + { + return joint_position_interface_ ? joint_position_interface_.get_name() : + joint_velocity_interface_ ? joint_velocity_interface_.get_name() : + joint_command_interface_ ? joint_command_interface_.get_name() : + std::string(); + } + + /// Sub-class implementation of limit enforcing policy. + virtual void enforce_limits(const rclcpp::Duration & period) = 0; + + /// Clear stored state, causing it to reset next iteration. + virtual void reset() + { + prev_pos_ = std::numeric_limits::quiet_NaN(); + prev_vel_ = 0.0; + } + +protected: + hardware_interface::StateInterface joint_position_interface_; + hardware_interface::StateInterface joint_velocity_interface_; + hardware_interface::CommandInterface & joint_command_interface_; + joint_limits_interface::JointLimits limits_; + + // stored state - track position and velocity of last update + double prev_pos_; + double prev_vel_; + + /// Return velocity for limit calculations. + /** + * @param period Time since last measurement + * @return the velocity, from state if available, otherwise from previous position history. + */ + double get_velocity(const rclcpp::Duration & period) const + { + // if we have a handle to a velocity state we can directly return state velocity + // otherwise we will estimate velocity from previous position (command or state) + return joint_velocity_interface_ ? + joint_velocity_interface_.get_value() : + (joint_position_interface_.get_value() - prev_pos_) / period.seconds(); + } +}; + + +/** The base class of limit handles for enforcing position, velocity, and effort limits of + * an effort-controlled joint that has soft-limits. + */ +class JointSoftLimitsHandle : public JointLimitHandle +{ +public: +// JointSoftLimitsHandle() {} + + JointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits, + const SoftJointLimits & soft_limits) + : JointLimitHandle(joint_position_interface, joint_command_interface, limits), + soft_limits_(soft_limits) + {} + + JointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits, + const SoftJointLimits & soft_limits) + : JointLimitHandle(joint_position_interface, joint_velocity_interface, + joint_command_interface, limits), + soft_limits_(soft_limits) + {} + +protected: + joint_limits_interface::SoftJointLimits soft_limits_; +}; + + +/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have + soft limits. */ +class PositionJointSaturationHandle : public JointLimitHandle +{ +public: +// PositionJointSaturationHandle() {} + + PositionJointSaturationHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const JointLimits & limits) + : JointLimitHandle(joint_position_interface, joint_command_interface, limits) + { + if (limits_.has_position_limits) { + min_pos_limit_ = limits_.min_position; + max_pos_limit_ = limits_.max_position; + } else { + min_pos_limit_ = -std::numeric_limits::max(); + max_pos_limit_ = std::numeric_limits::max(); + } + } + +/// Enforce position and velocity limits for a joint that is not subject to soft limits. +/** + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) + { + if (std::isnan(prev_pos_)) { + prev_pos_ = joint_position_interface_.get_value(); + } + + double min_pos, max_pos; + if (limits_.has_velocity_limits) { + // enforce velocity limits + // set constraints on where the position can be based on the + // max velocity times seconds since last update + const double delta_pos = limits_.max_velocity * period.seconds(); + min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); + max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); + } else { + // no velocity limit, so position is simply limited to set extents (our imposed soft limits) + min_pos = min_pos_limit_; + max_pos = max_pos_limit_; + } + + // clamp command position to our computed min/max position + const double cmd = rcppmath::clamp(joint_command_interface_.get_value(), min_pos, max_pos); + joint_command_interface_.set_value(cmd); + + prev_pos_ = cmd; + } + +private: + double min_pos_limit_, max_pos_limit_; +}; + +/// A handle used to enforce position and velocity limits of a position-controlled joint. +/** + * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least + * amount of requisites on the underlying hardware platform. + * This lowers considerably the entry barrier to use it, but also implies some limitations. + * + * Requisites + * - Position (for non-continuous joints) and velocity limits specification. + * - Soft limits specification. The \c k_velocity parameter is \e not used. + * + * Open loop nature + * + * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for + * validity without relying on the actual position/velocity values. + * + * - Actual position values are \e not used because in some platforms there might be a substantial lag + * between sending a command and executing it (propagate command to hardware, reach control objective, + * read from hardware). + * + * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose + * trustworthy velocity measurements, or none at all. + * + * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large + * position tracking errors. Only the command is guaranteed to comply with the limits specification. + * + * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command + * velocity. + */ + +// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? +class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: +// PositionJointSoftLimitsHandle() +// {} + + PositionJointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_command_interface, limits, soft_limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + /// Enforce position and velocity limits for a joint subject to soft limits. + /** + * If the joint has no position limits (eg. a continuous joint), only velocity limits will be + * enforced. + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + assert(period.seconds() > 0.0); + + // Current position + if (std::isnan(prev_pos_)) { + // Happens only once at initialization + prev_pos_ = joint_position_interface_.get_value(); + } + const double pos = prev_pos_; + + // Velocity bounds + double soft_min_vel; + double soft_max_vel; + + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit + soft_min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -limits_.max_velocity, + limits_.max_velocity); + } else { + // No position limits, eg. continuous joints + soft_min_vel = -limits_.max_velocity; + soft_max_vel = limits_.max_velocity; + } + + // Position bounds + const double dt = period.seconds(); + double pos_low = pos + soft_min_vel * dt; + double pos_high = pos + soft_max_vel * dt; + + if (limits_.has_position_limits) { + // This extra measure safeguards against pathological cases, like when the soft limit lies + // beyond the hard limit + pos_low = std::max(pos_low, limits_.min_position); + pos_high = std::min(pos_high, limits_.max_position); + } + + // Saturate position command according to bounds + const double pos_cmd = rcppmath::clamp( + joint_command_interface_.get_value(), + pos_low, + pos_high); + joint_command_interface_.set_value(pos_cmd); + + // Cache variables + // todo: shouldn't this just be pos_cmd? why call into the command handle to + // get what we have in the above line? + prev_pos_ = joint_command_interface_.get_value(); + } +}; + +/** + * A handle used to enforce position, velocity, and effort limits of an effort-controlled + * joint that does not have soft limits. + */ +class EffortJointSaturationHandle : public JointLimitHandle +{ +public: +// EffortJointSaturationHandle() {} + + EffortJointSaturationHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle(joint_position_interface, joint_velocity_interface, + joint_command_interface, limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no efforts limits specification."); + } + } + + EffortJointSaturationHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits) + : EffortJointSaturationHandle( + joint_position_interface, + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + joint_command_interface, + limits) + { + } + + /** + * Enforce position, velocity, and effort limits for a joint that is not subject + * to soft limits. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + double min_eff = -limits_.max_effort; + double max_eff = limits_.max_effort; + + if (limits_.has_position_limits) { + const double pos = joint_position_interface_.get_value(); + if (pos < limits_.min_position) { + min_eff = 0.0; + } else if (pos > limits_.max_position) { + max_eff = 0.0; + } + } + + const double vel = get_velocity(period); + if (vel < -limits_.max_velocity) { + min_eff = 0.0; + } else if (vel > limits_.max_velocity) { + max_eff = 0.0; + } + + double clamped = rcppmath::clamp(joint_command_interface_.get_value(), min_eff, max_eff); + joint_command_interface_.set_value(clamped); + } +}; + +/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. +// TODO(anyone): This class is untested!. Update unit tests accordingly. +class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: +// EffortJointSoftLimitsHandle() {} + + EffortJointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, + joint_command_interface, limits, soft_limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no effort limits specification."); + } + } + + EffortJointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : EffortJointSoftLimitsHandle( + joint_position_interface, + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + joint_command_interface, + limits, + soft_limits) + { + } + + /// Enforce position, velocity and effort limits for a joint subject to soft limits. + /** + * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits + * will be enforced. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + // Current state + const double pos = joint_position_interface_.get_value(); + const double vel = get_velocity(period); + + // Velocity bounds + double soft_min_vel; + double soft_max_vel; + + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit + soft_min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -limits_.max_velocity, + limits_.max_velocity); + } else { + // No position limits, eg. continuous joints + soft_min_vel = -limits_.max_velocity; + soft_max_vel = limits_.max_velocity; + } + + // Effort bounds depend on the velocity and effort bounds + const double soft_min_eff = rcppmath::clamp( + -soft_limits_.k_velocity * (vel - soft_min_vel), + -limits_.max_effort, + limits_.max_effort); + + const double soft_max_eff = rcppmath::clamp( + -soft_limits_.k_velocity * (vel - soft_max_vel), + -limits_.max_effort, + limits_.max_effort); + + // Saturate effort command according to bounds + const double eff_cmd = rcppmath::clamp( + joint_command_interface_.get_value(), + soft_min_eff, + soft_max_eff); + joint_command_interface_.set_value(eff_cmd); + } +}; + + +/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. +class VelocityJointSaturationHandle : public JointLimitHandle +{ +public: +// VelocityJointSaturationHandle() {} + + VelocityJointSaturationHandle( + const hardware_interface::StateInterface & joint_velocity_interface, // currently unused + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle( + hardware_interface::StateInterface("position"), + joint_velocity_interface, + joint_command_interface, + limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + VelocityJointSaturationHandle( + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits) + : JointLimitHandle(hardware_interface::StateInterface(hardware_interface::HW_IF_POSITION), + hardware_interface::StateInterface(hardware_interface::HW_IF_VELOCITY), + joint_command_interface, limits) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + /// Enforce joint velocity and acceleration limits. + /** + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) override + { + // Velocity bounds + double vel_low; + double vel_high; + + if (limits_.has_acceleration_limits) { + assert(period.seconds() > 0.0); + const double dt = period.seconds(); + + vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); + vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); + } else { + vel_low = -limits_.max_velocity; + vel_high = limits_.max_velocity; + } + + // Saturate velocity command according to limits + const double vel_cmd = rcppmath::clamp( + joint_command_interface_.get_value(), + vel_low, + vel_high); + joint_command_interface_.set_value(vel_cmd); + + // Cache variables + prev_vel_ = joint_command_interface_.get_value(); + } +}; + +/** + * A handle used to enforce position, velocity, and acceleration limits of a + * velocity-controlled joint. + */ +class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle +{ +public: +// VelocityJointSoftLimitsHandle() {} + + VelocityJointSoftLimitsHandle( + const hardware_interface::StateInterface & joint_position_interface, + const hardware_interface::StateInterface & joint_velocity_interface, + hardware_interface::CommandInterface & joint_command_interface, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : JointSoftLimitsHandle(joint_position_interface, joint_velocity_interface, + joint_command_interface, limits, soft_limits) + { + if (limits.has_velocity_limits) { + max_vel_limit_ = limits.max_velocity; + } else { + max_vel_limit_ = std::numeric_limits::max(); + } + } + + /** + * Enforce position, velocity, and acceleration limits for a velocity-controlled joint + * subject to soft limits. + * + * \param[in] period Control period. + */ + void enforce_limits(const rclcpp::Duration & period) + { + double min_vel, max_vel; + if (limits_.has_position_limits) { + // Velocity bounds depend on the velocity limit and the proximity to the position limit. + const double pos = joint_position_interface_.get_value(); + min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -max_vel_limit_, max_vel_limit_); + max_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.max_position), + -max_vel_limit_, max_vel_limit_); + } else { + min_vel = -max_vel_limit_; + max_vel = max_vel_limit_; + } + + if (limits_.has_acceleration_limits) { + const double vel = get_velocity(period); + const double delta_t = period.seconds(); + min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); + max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); + } + + joint_command_interface_.set_value( + rcppmath::clamp( + joint_command_interface_.get_value(), + min_vel, max_vel)); + } + +private: + double max_vel_limit_; +}; + +// TODO(anyone): Port this to ROS 2 +// //** +// * Interface for enforcing joint limits. +// * +// * \tparam HandleType %Handle type. Must implement the following methods: +// * \code +// * void enforce_limits(); +// * std::string get_name() const; +// * \endcode +// */ +// template +// class joint_limits_interface::JointLimitsInterface +// : public hardware_interface::ResourceManager +// { +// public: +// HandleType getHandle(const std::string & name) +// { +// // Rethrow exception with a meaningful type +// try { +// return this->hardware_interface::ResourceManager::getHandle(name); +// } catch (const std::logic_error & e) { +// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); +// } +// } +// +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Enforce limits for all managed handles. */ +// void enforce_limits(const rclcpp::Duration & period) +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.enforce_limits(period); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on a position-controlled joint through saturation. */ +// class PositionJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// public: +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Reset all managed handles. */ +// void reset() +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.reset(); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ +// class PositionJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// public: +// /** \name Real-Time Safe Functions +// *\{*/ +// /** Reset all managed handles. */ +// void reset() +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.reset(); +// } +// } +// /*\}*/ +// }; +// +// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ +// class EffortJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ +// class EffortJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ +// class VelocityJointSaturationInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +// +// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ +// class VelocityJointSoftLimitsInterface +// : public joint_limits_interface::JointLimitsInterface +// { +// }; +} // namespace joint_limits_interface + +#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp similarity index 100% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_rosparam.hpp similarity index 100% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits_rosparam.hpp diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp b/hardware_interface/include/joint_limits_interface/joint_limits_urdf.hpp similarity index 100% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp rename to hardware_interface/include/joint_limits_interface/joint_limits_urdf.hpp diff --git a/joint_limits_interface/mainpage.dox b/hardware_interface/joint_limits.dox similarity index 100% rename from joint_limits_interface/mainpage.dox rename to hardware_interface/joint_limits.dox diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 35bdd8c1af..73b4fdf60e 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -13,6 +13,7 @@ pluginlib rcpputils tinyxml2_vendor + urdf rcutils rcutils diff --git a/hardware_interface/src/joint_limits/simple_joint_limiter.cpp b/hardware_interface/src/joint_limits/simple_joint_limiter.cpp new file mode 100644 index 0000000000..36b2c738b5 --- /dev/null +++ b/hardware_interface/src/joint_limits/simple_joint_limiter.cpp @@ -0,0 +1,164 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include "joint_limits/simple_joint_limiter.hpp" + +#include +#include +#include +#include + +#include "rcppmath/clamp.hpp" + +namespace joint_limits +{ +SimpleJointLimiter::SimpleJointLimiter( + JointLimits & limits, std::vector & state_interfaces, + std::vector & command_interfaces) +: JointLimiterInterface(limits, state_interfaces, command_interfaces) +{ + // Position state interface has to be always present and not virtual + if ( + std::find_if( + state_interfaces.begin(), state_interfaces.end(), + [](const hardware_interface::StateInterface & interface) { + return interface.get_name() == hardware_interface::HW_IF_POSITION; + }) == state_interfaces.end()) + { + throw std::runtime_error( + "Simple joint limiter requires position state interface for joint '" + get_name() + + "'."); + } + + if (has_effort_command_) { + if (!limits.has_velocity_limits) { + throw std::runtime_error( + "Cannot enforce limits on effort command interface for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) { + throw std::runtime_error( + "Cannot enforce limits on effort command interface for joint '" + get_name() + + "'. It has no efforts limits specification."); + } + if ( + std::find_if( + state_interfaces.begin(), state_interfaces.end(), + [](const hardware_interface::StateInterface & interface) { + return interface.get_name() == hardware_interface::HW_IF_VELOCITY; + }) != state_interfaces.end()) + { + has_velocity_state_ = true; + } + } + + if (limits_.has_position_limits) { + min_position_limit_ = limits_.min_position; + max_position_limit_ = limits_.max_position; + } else { + min_position_limit_ = -std::numeric_limits::max(); + max_position_limit_ = std::numeric_limits::max(); + } + if (limits_.has_velocity_limits) { + max_velocity_limit_ = limits_.max_velocity; + } else { + max_velocity_limit_ = std::numeric_limits::max(); + } + if (limits_.has_acceleration_limits) { + max_acceleration_limit_ = limits_.max_velocity; + } else { + max_acceleration_limit_ = std::numeric_limits::max(); + } +} + +void SimpleJointLimiter::enforce_effort_limits(const rclcpp::Duration & period) +{ + double min_effort = -limits_.max_effort; + double max_effort = limits_.max_effort; + + const double current_position = state_interfaces_[0].get_value(); + if (current_position < limits_.min_position) { + min_effort = 0.0; + } else if (current_position > limits_.max_position) { + max_effort = 0.0; + } + + double current_velocity = 0.0; + if (has_velocity_state_) { + current_velocity = state_interfaces_[1].get_value(); + } else { + current_velocity = (current_position - previous_position_) / period.seconds(); + previous_position_ = current_position; + } + + if (current_velocity < -max_velocity_limit_) { + min_effort = 0.0; + } else if (current_velocity > max_velocity_limit_) { + min_effort = 0.0; + } + + // Saturate effort command according to limits + command_interfaces_[0]->set_value( + rcppmath::clamp(command_interfaces_[0]->get_value(), min_effort, max_effort)); +} + +void SimpleJointLimiter::enforce_pos_vel_acc_limits(const rclcpp::Duration & period) +{ + double min_position, max_position; + double min_velocity = -max_velocity_limit_; + double max_velocity = max_velocity_limit_; + double min_acceleration = -max_acceleration_limit_; + double max_acceleration = max_acceleration_limit_; + const double dt = period.seconds(); + + // Make trivial dynamic clamping + if (previous_position_ == min_position_limit_) { + min_velocity = 0.0; + } else if (previous_position_ == max_position_limit_) { + max_velocity = 0.0; + } + if (previous_velocity_ <= min_velocity) { + min_acceleration = 0.0; + } else if (previous_velocity_ >= max_velocity) { + max_acceleration = 0.0; + } + + // enforce acceleration limits - set velocity constants + // the velocity is based on the max acceleration times seconds since last update + min_velocity = std::max(previous_velocity_ + min_acceleration * dt, min_velocity); + max_velocity = std::min(previous_velocity_ + max_acceleration * dt, max_velocity); + + min_position = std::max(previous_position_ + min_velocity * dt, min_position_limit_); + max_position = std::min(previous_position_ + max_velocity * dt, max_position_limit_); + + // Saturate acceleration command according to limits + command_interfaces_[2]->set_value( + rcppmath::clamp(command_interfaces_[2]->get_value(), min_acceleration, max_acceleration)); + + // Saturate velocity command according to limits + command_interfaces_[1]->set_value( + rcppmath::clamp(command_interfaces_[1]->get_value(), min_velocity, max_velocity)); + // Cache variables + previous_velocity_ = command_interfaces_[1]->get_value(); + + // Saturate position command according to limits + command_interfaces_[0]->set_value( + rcppmath::clamp(command_interfaces_[0]->get_value(), min_position_limit_, max_position_limit_)); + // Cache variables + previous_position_ = command_interfaces_[0]->get_value(); +} + +} // namespace joint_limits diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 3b208cc395..3af2f388a9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -231,12 +231,12 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin std::string("Command interface with '") + key + "' does not exist"); } - std::lock_guard lg(resource_lock_); if (command_interface_is_claimed(key)) { throw std::runtime_error( std::string("Command interface with '") + key + "' is already claimed"); } + std::lock_guard lg(resource_lock_); claimed_command_interface_map_[key] = true; return LoanedCommandInterface( resource_storage_->command_interface_map_.at(key), diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp similarity index 67% rename from joint_limits_interface/test/joint_limits_interface_test.cpp rename to hardware_interface/test/joint_limits/joint_limits_interface_test.cpp index 7d14410c9d..295603621f 100644 --- a/joint_limits_interface/test/joint_limits_interface_test.cpp +++ b/hardware_interface/test/joint_limits/joint_limits_interface_test.cpp @@ -64,10 +64,8 @@ class JointLimitsTest : pos(0.0), vel(0.0), eff(0.0), cmd(0.0), name("joint_name"), period(0, 100000000), - cmd_handle(hardware_interface::JointHandle(name, "position_command", &cmd)), - pos_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_POSITION, &pos)), - vel_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_VELOCITY, &vel)), - eff_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_EFFORT, &eff)) + position_command_handle( + name, hardware_interface::HW_IF_POSITION, &cmd) { limits.has_position_limits = true; limits.min_position = -1.0; @@ -90,10 +88,29 @@ class JointLimitsTest double pos, vel, eff, cmd; std::string name; rclcpp::Duration period; - hardware_interface::JointHandle cmd_handle; - hardware_interface::JointHandle pos_handle, vel_handle, eff_handle; joint_limits_interface::JointLimits limits; joint_limits_interface::SoftJointLimits soft_limits; + hardware_interface::CommandInterface position_command_handle; + + inline hardware_interface::CommandInterface & command_handle() + { + return position_command_handle; + } + + inline hardware_interface::StateInterface position_handle() + { + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_POSITION, &pos); + } + + inline hardware_interface::StateInterface velocity_handle() + { + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_VELOCITY, &vel); + } + + inline hardware_interface::StateInterface effort_handle() + { + return hardware_interface::StateInterface(name, hardware_interface::HW_IF_EFFORT, &eff); + } }; class JointLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; @@ -104,7 +121,7 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) joint_limits_interface::JointLimits limits_bad; EXPECT_THROW( joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, + position_handle(), command_handle(), limits_bad, soft_limits), joint_limits_interface::JointLimitsInterfaceException); @@ -112,7 +129,7 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) // descriptive try { joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); + position_handle(), command_handle(), limits_bad, soft_limits); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -123,14 +140,14 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) limits_bad.has_effort_limits = true; EXPECT_THROW( joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, + position_handle(), command_handle(), limits_bad, soft_limits), joint_limits_interface::JointLimitsInterfaceException); // Print error messages. Requires manual output inspection, // but exception message should be descriptive try { joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); + position_handle(), command_handle(), limits_bad, soft_limits); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -141,14 +158,14 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) limits_bad.has_velocity_limits = true; EXPECT_THROW( joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, + position_handle(), command_handle(), limits_bad, soft_limits), joint_limits_interface::JointLimitsInterfaceException); // Print error messages. Requires manual output inspection, but exception message should // be descriptive try { joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); + position_handle(), command_handle(), limits_bad, soft_limits); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -158,13 +175,14 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) joint_limits_interface::JointLimits limits_bad; EXPECT_THROW( joint_limits_interface::VelocityJointSaturationHandle( - pos_handle, cmd_handle, + position_handle(), command_handle(), limits_bad), joint_limits_interface::JointLimitsInterfaceException); // Print error messages. Requires manual output inspection, but exception message should // be descriptive try { - joint_limits_interface::VelocityJointSaturationHandle(pos_handle, cmd_handle, limits_bad); + joint_limits_interface::VelocityJointSaturationHandle( + position_handle(), command_handle(), limits_bad); } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); } @@ -172,13 +190,13 @@ TEST_F(JointLimitsHandleTest, HandleConstruction) EXPECT_NO_THROW( joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits, soft_limits)); + position_handle(), command_handle(), limits, soft_limits)); EXPECT_NO_THROW( joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits, soft_limits)); + position_handle(), command_handle(), limits, soft_limits)); EXPECT_NO_THROW( joint_limits_interface::VelocityJointSaturationHandle( - pos_handle, cmd_handle, limits)); + position_handle(), command_handle(), limits)); } class PositionJointSoftLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; @@ -194,55 +212,55 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) // Move slower than maximum velocity { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = max_increment / 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = -max_increment / 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); } // Move at maximum velocity { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = max_increment; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = -max_increment; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); } // Try to move faster than the maximum velocity, enforce velocity limits { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = 2.0 * max_increment; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(max_increment, cmd_handle.get_value(), EPS); + EXPECT_NEAR(max_increment, command_handle().get_value(), EPS); } { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); cmd = -2.0 * max_increment; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(-max_increment, cmd_handle.get_value(), EPS); + EXPECT_NEAR(-max_increment, command_handle().get_value(), EPS); } } @@ -256,79 +274,79 @@ TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) // Current position == upper soft limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Can't get any closer to hard limit (zero max velocity) pos = soft_limits.max_position; // Try to get closer to the hard limit - cmd_handle.set_value(limits.max_position); + command_handle().set_value(limits.max_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); + EXPECT_NEAR(position_handle().get_value(), command_handle().get_value(), EPS); // OK to move away from hard limit // Try to go to workspace center - cmd_handle.set_value(workspace_center); + command_handle().set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); } // Current position == lower soft limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Can't get any closer to hard limit (zero min velocity) pos = soft_limits.min_position; // Try to get closer to the hard limit - cmd_handle.set_value(limits.min_position); + command_handle().set_value(limits.min_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); + EXPECT_NEAR(position_handle().get_value(), command_handle().get_value(), EPS); // OK to move away from hard limit // Try to go to workspace center - cmd_handle.set_value(workspace_center); + command_handle().set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); } // Current position > upper soft limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Can't get any closer to hard limit (negative max velocity) // Halfway between soft and hard limit pos = (soft_limits.max_position + limits.max_position) / 2.0; // Try to get closer to the hard limit - cmd_handle.set_value(limits.max_position); + command_handle().set_value(limits.max_position); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); // OK to move away from hard limit // Try to go to workspace center - cmd_handle.set_value(workspace_center); + command_handle().set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_GT(position_handle().get_value(), command_handle().get_value()); } // Current position < lower soft limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Can't get any closer to hard limit (positive min velocity) // Halfway between soft and hard limit pos = (soft_limits.min_position + limits.min_position) / 2.0; // Try to get closer to the hard limit - cmd_handle.set_value(limits.min_position); + command_handle().set_value(limits.min_position); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); // OK to move away from hard limit // Try to go to workspace center - cmd_handle.set_value(workspace_center); + command_handle().set_value(workspace_center); limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); + EXPECT_LT(position_handle().get_value(), command_handle().get_value()); } } @@ -343,29 +361,29 @@ TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) // Current position == higher hard limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Hit hard limit // On hard limit pos = limits.max_position; // Way beyond hard limit - cmd_handle.set_value(2.0 * limits.max_position); + command_handle().set_value(2.0 * limits.max_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_position, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.max_position, command_handle().get_value(), EPS); } // Current position == lower hard limit { joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); + position_handle(), command_handle(), limits, soft_limits); // Hit hard limit // On hard limit pos = limits.min_position; // Way beyond hard limit - cmd_handle.set_value(2.0 * limits.min_position); + command_handle().set_value(2.0 * limits.min_position); limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.min_position, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.min_position, command_handle().get_value(), EPS); } } @@ -375,43 +393,43 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceVelocityBounds) { // Test setup joint_limits_interface::VelocityJointSaturationHandle limits_handle( - pos_handle, cmd_handle, limits); + position_handle(), command_handle(), limits); pos = 0.0; double cmd; // Velocity within bounds cmd = limits.max_velocity / 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); cmd = -limits.max_velocity / 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); // Velocity at bounds cmd = limits.max_velocity; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); cmd = -limits.max_velocity; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); + EXPECT_NEAR(cmd, command_handle().get_value(), EPS); // Velocity beyond bounds cmd = 2.0 * limits.max_velocity; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.max_velocity, command_handle().get_value(), EPS); cmd = -2.0 * limits.max_velocity; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); - EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); + EXPECT_NEAR(-limits.max_velocity, command_handle().get_value(), EPS); } TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) @@ -420,7 +438,7 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) limits.has_acceleration_limits = true; limits.max_acceleration = limits.max_velocity / period.seconds(); joint_limits_interface::VelocityJointSaturationHandle limits_handle( - pos_handle, cmd_handle, limits); + position_handle(), command_handle(), limits); pos = 0.0; double cmd; @@ -429,57 +447,57 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) // Positive velocity // register last command - cmd_handle.set_value(limits.max_velocity / 2.0); + command_handle().set_value(limits.max_velocity / 2.0); // make sure the prev_cmd is registered // without triggering the acceleration limits limits_handle.enforce_limits(long_enough); // Try to go beyond +max velocity cmd = limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); // Max velocity bounded by velocity limit - EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.max_velocity, command_handle().get_value(), EPS); // register last command - cmd_handle.set_value(limits.max_velocity / 2.0); + command_handle().set_value(limits.max_velocity / 2.0); // make sure the prev_cmd is registered // without triggering the acceleration limits limits_handle.enforce_limits(long_enough); // Try to go beyond -max velocity cmd = -limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); // Max velocity bounded by acceleration limit - EXPECT_NEAR(-limits.max_velocity / 2.0, cmd_handle.get_value(), EPS); + EXPECT_NEAR(-limits.max_velocity / 2.0, command_handle().get_value(), EPS); // Negative velocity // register last command - cmd_handle.set_value(-limits.max_velocity / 2.0); + command_handle().set_value(-limits.max_velocity / 2.0); // make sure the prev_cmd is registered // without triggering the acceleration limits limits_handle.enforce_limits(long_enough); // Try to go beyond +max velocity cmd = limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); // Max velocity bounded by acceleration limit - EXPECT_NEAR(limits.max_velocity / 2.0, cmd_handle.get_value(), EPS); + EXPECT_NEAR(limits.max_velocity / 2.0, command_handle().get_value(), EPS); // register last command - cmd_handle.set_value(-limits.max_velocity / 2.0); + command_handle().set_value(-limits.max_velocity / 2.0); // make sure the prev_cmd is registered // without triggering the acceleration limits limits_handle.enforce_limits(long_enough); // Try to go beyond -max velocity cmd = -limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); + command_handle().set_value(cmd); limits_handle.enforce_limits(period); // Max velocity bounded by velocity limit - EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); + EXPECT_NEAR(-limits.max_velocity, command_handle().get_value(), EPS); } class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test @@ -488,35 +506,41 @@ class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test JointLimitsInterfaceTest() : JointLimitsTest(), pos2(0.0), vel2(0.0), eff2(0.0), cmd2(0.0), - name2("joint2_name"), - cmd2_handle(std::make_shared( - name2, "position_command", - &cmd2)), - pos2_handle(std::make_shared( - name2, - hardware_interface::HW_IF_POSITION, &pos2)), - vel2_handle(std::make_shared( - name2, - hardware_interface::HW_IF_VELOCITY, &vel2)), - eff2_handle(std::make_shared( - name2, - hardware_interface::HW_IF_EFFORT, &eff2)) + name2("joint2_name") {} protected: double pos2, vel2, eff2, cmd2; std::string name2; - std::shared_ptr cmd2_handle; - std::shared_ptr pos2_handle, vel2_handle, eff2_handle; + + inline hardware_interface::CommandInterface command2_handle() + { + return hardware_interface::CommandInterface(name2, hardware_interface::HW_IF_POSITION, &cmd2); + } + + inline hardware_interface::StateInterface position2_handle() + { + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_POSITION, &pos2); + } + + inline hardware_interface::StateInterface velocity2_handle() + { + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_VELOCITY, &vel2); + } + + inline hardware_interface::StateInterface effort2_handle() + { + return hardware_interface::StateInterface(name2, hardware_interface::HW_IF_EFFORT, &eff2); + } }; // TEST_F(JointLimitsInterfaceTest, InterfaceRegistration) // { // // Populate interface // joint_limits_interface::PositionJointSoftLimitsHandle limits_handle1( -// pos_handle, cmd_handle, limits, soft_limits); +// position_handle(), command_handle(), limits, soft_limits); // joint_limits_interface::PositionJointSoftLimitsHandle limits_handle2( -// pos_handle2, cmd_handle2, limits, soft_limits); +// position2_handle(), command2_handle(), limits, soft_limits); // // joint_limits_interface::PositionJointSoftLimitsInterface iface; // iface.registerHandle(limits_handle1); @@ -545,11 +569,11 @@ class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test // // Halfway between soft and hard limit // pos = pos2 = (soft_limits.max_position + limits.max_position) / 2.0; // // Try to get closer to the hard limit -// cmd_handle.set_value(limits.max_position); -// cmd_handle2.set_cmd(limits.max_position); +// command_handle().set_value(limits.max_position); +// command2_handle().set_cmd(limits.max_position); // iface.enforce_limits(period); -// EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); -// EXPECT_GT(cmd_handle2.getPosition(), cmd_handle2.get_cmd()); +// EXPECT_GT(position_handle().get_value(), command_handle().get_value()); +// EXPECT_GT(command2_handle().getPosition(), command2_handle().get_cmd()); // } // #if 0 // todo: implement the interfaces @@ -557,7 +581,7 @@ TEST_F(JointLimitsHandleTest, ResetSaturationInterface) { // Populate interface joint_limits_interface::PositionJointSaturationHandle limits_handle1 - (pos_handle, cmd_handle, limits); + (position_handle(), command_handle(), limits); PositionJointSaturationInterface iface; iface.registerHandle(limits_handle1); @@ -566,24 +590,24 @@ TEST_F(JointLimitsHandleTest, ResetSaturationInterface) const double max_increment = period.seconds() * limits.max_velocity; - cmd_handle.set_value(limits.max_position); + command_handle().set_value(limits.max_position); iface.enforce_limits(period); - EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); + EXPECT_NEAR(command_handle().get_value(), max_increment, EPS); iface.reset(); pos = limits.max_position; - cmd_handle.set_value(limits.max_position); + command_handle().set_value(limits.max_position); iface.enforce_limits(period); - EXPECT_NEAR(cmd_handle.get_value(), limits.max_position, EPS); + EXPECT_NEAR(command_handle().get_value(), limits.max_position, EPS); } #endif // TEST_F(JointLimitsHandleTest, ResetSoftLimitsInterface) // { // // Populate interface -// PositionJointSoftLimitsHandle limits_handle1(cmd_handle, limits, soft_limits); +// PositionJointSoftLimitsHandle limits_handle1(command_handle(), limits, soft_limits); // // PositionJointSoftLimitsInterface iface; // iface.registerHandle(limits_handle1); @@ -592,17 +616,17 @@ TEST_F(JointLimitsHandleTest, ResetSaturationInterface) // // const double max_increment = period.seconds() * limits.max_velocity; // -// cmd_handle.set_value(limits.max_position); +// command_handle().set_value(limits.max_position); // iface.enforce_limits(period); // -// EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); +// EXPECT_NEAR(command_handle().get_value(), max_increment, EPS); // // iface.reset(); // pos = limits.max_position; -// cmd_handle.set_value(soft_limits.max_position); +// command_handle().set_value(soft_limits.max_position); // iface.enforce_limits(period); // -// EXPECT_NEAR(cmd_handle.get_value(), soft_limits.max_position, EPS); +// EXPECT_NEAR(command_handle().get_value(), soft_limits.max_position, EPS); // // } diff --git a/joint_limits_interface/test/joint_limits_rosparam.launch.py b/hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py similarity index 93% rename from joint_limits_interface/test/joint_limits_rosparam.launch.py rename to hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py index 68670574a3..ffbc311d66 100644 --- a/joint_limits_interface/test/joint_limits_rosparam.launch.py +++ b/hardware_interface/test/joint_limits/joint_limits_rosparam.launch.py @@ -27,10 +27,10 @@ def generate_test_description(): - joint_limits_interface_path = get_package_share_directory('joint_limits_interface') + joint_limits_interface_path = get_package_share_directory('hardware_interface') node_under_test = Node( - package='joint_limits_interface', + package='hardware_interface', executable='joint_limits_rosparam_test', output='screen', parameters=[os.path.join(joint_limits_interface_path, diff --git a/joint_limits_interface/test/joint_limits_rosparam.yaml b/hardware_interface/test/joint_limits/joint_limits_rosparam.yaml similarity index 100% rename from joint_limits_interface/test/joint_limits_rosparam.yaml rename to hardware_interface/test/joint_limits/joint_limits_rosparam.yaml diff --git a/joint_limits_interface/test/joint_limits_rosparam_test.cpp b/hardware_interface/test/joint_limits/joint_limits_rosparam_test.cpp similarity index 99% rename from joint_limits_interface/test/joint_limits_rosparam_test.cpp rename to hardware_interface/test/joint_limits/joint_limits_rosparam_test.cpp index ce2191f073..90195736d9 100644 --- a/joint_limits_interface/test/joint_limits_rosparam_test.cpp +++ b/hardware_interface/test/joint_limits/joint_limits_rosparam_test.cpp @@ -28,7 +28,8 @@ class JointLimitsRosParamTest : public ::testing::Test void SetUp() { rclcpp::NodeOptions node_options; - node_options.allow_undeclared_parameters(true) + node_options + .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true); node_ = rclcpp::Node::make_shared("JointLimitsRosparamTestNode", node_options); diff --git a/joint_limits_interface/test/joint_limits_urdf_test.cpp b/hardware_interface/test/joint_limits/joint_limits_urdf_test.cpp similarity index 100% rename from joint_limits_interface/test/joint_limits_urdf_test.cpp rename to hardware_interface/test/joint_limits/joint_limits_urdf_test.cpp diff --git a/hardware_interface/test/joint_limits/test_joint_limiter_interface.cpp b/hardware_interface/test/joint_limits/test_joint_limiter_interface.cpp new file mode 100644 index 0000000000..ef2c5ca5aa --- /dev/null +++ b/hardware_interface/test/joint_limits/test_joint_limiter_interface.cpp @@ -0,0 +1,126 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include + +#include "hardware_interface/handle.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rcppmath/clamp.hpp" + +// Floating-point value comparison threshold +const double EPS = 1e-12; + +TEST(SaturateTest, Saturate) +{ + const double min = -1.0; + const double max = 2.0; + double val; + + val = -0.5; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = 0.5; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = -1.0; + EXPECT_NEAR(val, rcppmath::clamp(min, min, max), EPS); + + val = -2.0; + EXPECT_NEAR(min, rcppmath::clamp(val, min, max), EPS); + + val = 2.0; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = 3.0; + EXPECT_NEAR(max, rcppmath::clamp(val, min, max), EPS); +} + +class TestJointLimiter : public joint_limits::JointLimiterInterface +{ +public: + TestJointLimiter( + joint_limits::JointLimits & limits, + std::vector & state_interfaces, + std::vector & command_interfaces) + : JointLimiterInterface(limits, state_interfaces, command_interfaces) + {} + +protected: + void enforce_effort_limits(const rclcpp::Duration & /*period*/) override + {} + void enforce_pos_vel_acc_limits(const rclcpp::Duration & /*period*/) override + {} +}; + +class JointLimiterTest : public ::testing::Test +{ +public: + JointLimiterTest() + { + limits.has_position_limits = true; + limits.min_position = -1.0; + limits.max_position = 1.0; + + limits.has_velocity_limits = true; + limits.max_velocity = 2.0; + + limits.has_effort_limits = true; + limits.max_effort = 8.0; + } + + void SetUp() + { + commands_storage_.resize(4, 0.0); + states_storage_.resize(4, 0.0); + } + + joint_limits::JointLimits limits; + + std::vector commands_storage_; + std::vector states_storage_; +}; + +TEST_F(JointLimiterTest, Construction) +{ + // Test throw on missing command interfaces + { + std::vector state_interfaces; + std::vector command_interfaces; + EXPECT_THROW( + TestJointLimiter(limits, state_interfaces, command_interfaces), std::runtime_error); + } + + // Test OK when at least one command interface is provided + { + joint_limits::JointLimits limits_bad; + std::vector state_interfaces; + std::vector command_interfaces; + EXPECT_THROW( + TestJointLimiter(limits_bad, state_interfaces, command_interfaces), std::runtime_error); + } + + // Test bad limits +// { +// joint_limits::JointLimits limits_bad; +// std::vector state_interfaces; +// std::vector command_interfaces; +// EXPECT_THROW( +// TestJointLimiter(limits_bad, state_interfaces, command_interfaces), std::runtime_error); +// } +} diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt deleted file mode 100644 index 5af32d44b8..0000000000 --- a/joint_limits_interface/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 3.5.0) -project(joint_limits_interface) - -find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(rclcpp REQUIRED) -find_package(urdf REQUIRED) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - - ament_lint_auto_find_test_dependencies() - - ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp) - target_include_directories(joint_limits_interface_test PUBLIC include) - ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp) - - add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) - target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) - ament_target_dependencies(joint_limits_rosparam_test rclcpp) - add_launch_test(test/joint_limits_rosparam.launch.py) - install( - TARGETS - joint_limits_rosparam_test - DESTINATION lib/${PROJECT_NAME} - ) - install( - FILES - test/joint_limits_rosparam.yaml - DESTINATION share/${PROJECT_NAME}/test - ) - - ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) - target_include_directories(joint_limits_urdf_test PUBLIC include) - ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) -endif() - -# Install headers -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_dependencies(rclcpp urdf) - -ament_package() diff --git a/joint_limits_interface/COLCON_IGNORE b/joint_limits_interface/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml deleted file mode 100644 index 2170268c12..0000000000 --- a/joint_limits_interface/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - joint_limits_interface - 0.0.0 - Interface for enforcing joint limits. - - Bence Magyar - Jordan Palacios - - Apache License 2.0 - - https://github.com/ros-controls/ros2_control/wiki - https://github.com/ros-controls/ros2_control/issues - https://github.com/ros-controls/ros2_control - - Adolfo Rodriguez Tsouroukdissian - - ament_cmake - - rclcpp - urdf - - hardware_interface - - hardware_interface - - ament_cmake_gtest - ament_lint_auto - ament_lint_common - hardware_interface - launch - launch_ros - launch_testing - launch_testing_ament_cmake - - - ament_cmake - -