diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 3e296cd584..8be804fa23 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -7,12 +7,16 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + pluginlib + realtime_tools rclcpp rclcpp_lifecycle + trajectory_msgs urdf ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(ament_cmake_gen_version_h REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -26,10 +30,42 @@ target_include_directories(joint_limits INTERFACE ) ament_target_dependencies(joint_limits INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_library(joint_limiter_interface SHARED + src/joint_limiter_interface.cpp +) +target_compile_features(joint_limiter_interface PUBLIC cxx_std_17) +target_include_directories(joint_limiter_interface PUBLIC + $ + $ +) +ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDING_DLL") + + +add_library(joint_saturation_limiter SHARED + src/joint_saturation_limiter.cpp +) +target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) +target_include_directories(joint_saturation_limiter PUBLIC + $ + $ +) +ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_saturation_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(generate_parameter_library REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) target_link_libraries(joint_limits_rosparam_test joint_limits) @@ -43,16 +79,31 @@ if(BUILD_TESTING) DESTINATION lib/joint_limits ) install( - FILES test/joint_limits_rosparam.yaml + FILES test/joint_limits_rosparam.yaml test/joint_saturation_limiter_param.yaml DESTINATION share/joint_limits/test ) + + add_rostest_with_parameters_gmock(test_joint_saturation_limiter test/test_joint_saturation_limiter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/joint_saturation_limiter_param.yaml + ) + target_include_directories(test_joint_saturation_limiter PRIVATE include) + target_link_libraries(test_joint_saturation_limiter joint_limiter_interface) + ament_target_dependencies( + test_joint_saturation_limiter + pluginlib + rclcpp + ) + endif() install( DIRECTORY include/ DESTINATION include/joint_limits ) -install(TARGETS joint_limits +install(TARGETS + joint_limits + joint_limiter_interface + joint_saturation_limiter EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp new file mode 100644 index 0000000000..ed2f269812 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -0,0 +1,250 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ + +#include +#include + +#include "joint_limits/joint_limits.hpp" +#include "joint_limits/joint_limits_rosparam.hpp" +#include "joint_limits/visibility_control.h" +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace joint_limits +{ +using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint; + +template +class JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC JointLimiterInterface() = default; + + JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default; + + /// Initialization of every JointLimiter. + /** + * Initialization of JointLimiter for defined joints with their names. + * Robot description topic provides a topic name where URDF of the robot can be found. + * This is needed to use joint limits from URDF (not implemented yet!). + * Override this method only if initialization and reading joint limits should be adapted. + * Otherwise, initialize your custom limiter in `on_limit` method. + * + * \param[in] joint_names names of joints where limits should be applied. + * \param[in] param_itf node parameters interface object to access parameters. + * \param[in] logging_itf node logging interface to log if error happens. + * \param[in] robot_description_topic string of a topic where robot description is accessible. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + const std::string & robot_description_topic = "/robot_description") + { + number_of_joints_ = joint_names.size(); + joint_names_ = joint_names; + joint_limits_.resize(number_of_joints_); + node_param_itf_ = param_itf; + node_logging_itf_ = logging_itf; + + bool result = true; + + // TODO(destogl): get limits from URDF + + // Initialize and get joint limits from parameter server + for (size_t i = 0; i < number_of_joints_; ++i) + { + if (!declare_parameters(joint_names[i], node_param_itf_, node_logging_itf_)) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Joint '%s': parameter declaration has failed", joint_names[i].c_str()); + result = false; + break; + } + if (!get_joint_limits(joint_names[i], node_param_itf_, node_logging_itf_, joint_limits_[i])) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str()); + result = false; + break; + } + RCLCPP_INFO( + node_logging_itf_->get_logger(), "Limits for joint %zu (%s) are:\n%s", i, + joint_names[i].c_str(), joint_limits_[i].to_string().c_str()); + } + updated_limits_.writeFromNonRT(joint_limits_); + + auto on_parameter_event_callback = [this](const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult set_parameters_result; + set_parameters_result.successful = true; + + std::vector updated_joint_limits = joint_limits_; + bool changed = false; + + for (size_t i = 0; i < number_of_joints_; ++i) + { + changed |= joint_limits::check_for_limits_update( + joint_names_[i], parameters, node_logging_itf_, updated_joint_limits[i]); + } + + if (changed) + { + updated_limits_.writeFromNonRT(updated_joint_limits); + RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!"); + } + + return set_parameters_result; + }; + + parameter_callback_ = + node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback); + + if (result) + { + result = on_init(); + } + + (void)robot_description_topic; // avoid linters output + + return result; + } + + /** + * Wrapper init method that accepts pointer to the Node. + * For details see other init method. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & robot_description_topic = "/robot_description") + { + return init( + joint_names, node->get_node_parameters_interface(), node->get_node_logging_interface(), + robot_description_topic); + } + + /** + * Wrapper init method that accepts pointer to the LifecycleNode. + * For details see other init method. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node, + const std::string & robot_description_topic = "/robot_description") + { + return init( + joint_names, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface(), robot_description_topic); + } + + JOINT_LIMITS_PUBLIC virtual bool configure(const JointLimitsStateDataType & current_joint_states) + { + return on_configure(current_joint_states); + } + + /** \brief Enforce joint limits to desired joint state for multiple physical quantities. + * + * Generic enforce method that calls implementation-specific `on_enforce` method. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool enforce( + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) + { + joint_limits_ = *(updated_limits_.readFromRT()); + return on_enforce(current_joint_states, desired_joint_states, dt); + } + + /** \brief Enforce joint limits to desired joint state for single physical quantity. + * + * Generic enforce method that calls implementation-specific `on_enforce` method. + * + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool enforce(std::vector & desired_joint_states) + { + joint_limits_ = *(updated_limits_.readFromRT()); + return on_enforce(desired_joint_states); + } + +protected: + /** \brief Method is realized by an implementation. + * + * Implementation-specific initialization of limiter's internal states and libraries. + * \returns true if initialization was successful, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_init() = 0; + + /** \brief Method is realized by an implementation. + * + * Implementation-specific configuration of limiter's internal states and libraries. + * \returns true if initialization was successful, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_configure( + const JointLimitsStateDataType & current_joint_states) = 0; + + /** \brief Method is realized by an implementation. + * + * Filter-specific implementation of the joint limits enforce algorithm for multiple dependent + * physical quantities. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_enforce( + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; + + /** \brief Method is realized by an implementation. + * + * Filter-specific implementation of the joint limits enforce algorithm for single physical + * quantity. + * This method might use "effort" limits since they are often used as wild-card. + * Check the documentation of the exact implementation for more details. + * + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_enforce(std::vector & desired_joint_states) = 0; + + size_t number_of_joints_; + std::vector joint_names_; + std::vector joint_limits_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_; + +private: + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; + realtime_tools::RealtimeBuffer> updated_limits_; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 2f32d49271..b23607f53d 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "joint_limits/joint_limits.hpp" #include "rclcpp/rclcpp.hpp" @@ -96,8 +97,6 @@ inline bool declare_parameters( auto_declare( param_itf, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_velocity_limits", false); - auto_declare( - param_itf, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); auto_declare( param_itf, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_acceleration_limits", false); @@ -237,7 +236,6 @@ inline bool get_joint_limits( !param_itf->has_parameter(param_base_name + ".min_position") && !param_itf->has_parameter(param_base_name + ".max_position") && !param_itf->has_parameter(param_base_name + ".has_velocity_limits") && - !param_itf->has_parameter(param_base_name + ".min_velocity") && !param_itf->has_parameter(param_base_name + ".max_velocity") && !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") && !param_itf->has_parameter(param_base_name + ".max_acceleration") && @@ -247,12 +245,7 @@ inline bool get_joint_limits( !param_itf->has_parameter(param_base_name + ".max_jerk") && !param_itf->has_parameter(param_base_name + ".has_effort_limits") && !param_itf->has_parameter(param_base_name + ".max_effort") && - !param_itf->has_parameter(param_base_name + ".angle_wraparound") && - !param_itf->has_parameter(param_base_name + ".has_soft_limits") && - !param_itf->has_parameter(param_base_name + ".k_position") && - !param_itf->has_parameter(param_base_name + ".k_velocity") && - !param_itf->has_parameter(param_base_name + ".soft_lower_limit") && - !param_itf->has_parameter(param_base_name + ".soft_upper_limit")) + !param_itf->has_parameter(param_base_name + ".angle_wraparound")) { RCLCPP_ERROR( logging_itf->get_logger(), @@ -421,6 +414,218 @@ inline bool get_joint_limits( lifecycle_node->get_node_logging_interface(), limits); } +/** + * Check if any of updated parameters are related to JointLimits. + * + * This method is intended to be used in the parameters update callback. + * It is recommended that it's result is temporarily stored and synchronized with the JointLimits + * structure in the main loop. + * + * \param[in] joint_name Name of the joint whose limits should be checked. + * \param[in] parameters List of the parameters that should be checked if they belong to this + * structure and that are used for updating the internal values. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] updated_limits structure with updated JointLimits. It is recommended that the + * currently used limits are stored into this structure before calling this method. + */ +inline bool check_for_limits_update( + const std::string & joint_name, const std::vector & parameters, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + JointLimits & updated_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + bool changed = false; + + // update first numerical values to make later checks for "has" limits members + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".min_position") + { + changed = updated_limits.min_position != parameter.get_value(); + updated_limits.min_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_position") + { + changed = updated_limits.max_position != parameter.get_value(); + updated_limits.max_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_velocity") + { + changed = updated_limits.max_velocity != parameter.get_value(); + updated_limits.max_velocity = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_acceleration") + { + changed = updated_limits.max_acceleration != parameter.get_value(); + updated_limits.max_acceleration = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_deceleration") + { + changed = updated_limits.max_deceleration != parameter.get_value(); + updated_limits.max_deceleration = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_jerk") + { + changed = updated_limits.max_jerk != parameter.get_value(); + updated_limits.max_jerk = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_effort") + { + changed = updated_limits.max_effort != parameter.get_value(); + updated_limits.max_effort = parameter.get_value(); + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_WARN(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".has_position_limits") + { + updated_limits.has_position_limits = parameter.get_value(); + if (updated_limits.has_position_limits) + { + if (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: Position limits can not be used, i.e., " + "'has_position_limits' flag can not be set, if 'min_position' " + "and 'max_position' are not set or not have valid double values."); + updated_limits.has_position_limits = false; + } + else if (updated_limits.min_position >= updated_limits.max_position) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: Position limits can not be used, i.e., " + "'has_position_limits' flag can not be set, if not " + "'min_position' < 'max_position'"); + updated_limits.has_position_limits = false; + } + else + { + changed = true; + } + } + } + else if (param_name == param_base_name + ".has_velocity_limits") + { + updated_limits.has_velocity_limits = parameter.get_value(); + if (updated_limits.has_velocity_limits && std::isnan(updated_limits.max_velocity)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_velocity_limits' flag can not be set if 'min_velocity' " + "and 'max_velocity' are not set or not have valid double values."); + updated_limits.has_velocity_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_acceleration_limits") + { + updated_limits.has_acceleration_limits = parameter.get_value(); + if (updated_limits.has_acceleration_limits && std::isnan(updated_limits.max_acceleration)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_acceleration_limits' flag can not be set if " + "'max_acceleration' is not set or not have valid double values."); + updated_limits.has_acceleration_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_deceleration_limits") + { + updated_limits.has_deceleration_limits = parameter.get_value(); + if (updated_limits.has_deceleration_limits && std::isnan(updated_limits.max_deceleration)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_deceleration_limits' flag can not be set if " + "'max_deceleration' is not set or not have valid double values."); + updated_limits.has_deceleration_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_jerk_limits") + { + updated_limits.has_jerk_limits = parameter.get_value(); + if (updated_limits.has_jerk_limits && std::isnan(updated_limits.max_jerk)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_jerk_limits' flag can not be set if 'max_jerk' is not set " + "or not have valid double values."); + updated_limits.has_jerk_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_effort_limits") + { + updated_limits.has_effort_limits = parameter.get_value(); + if (updated_limits.has_effort_limits && std::isnan(updated_limits.max_effort)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_effort_limits' flag can not be set if 'max_effort' is not " + "set or not have valid double values."); + updated_limits.has_effort_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".angle_wraparound") + { + updated_limits.angle_wraparound = parameter.get_value(); + if (updated_limits.angle_wraparound && updated_limits.has_position_limits) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'angle_wraparound' flag can not be set if " + "'has_position_limits' flag is set."); + updated_limits.angle_wraparound = false; + } + else + { + changed = true; + } + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_WARN( + logging_itf->get_logger(), "PARAMETER NOT UPDATED: Please use the right type: %s", + e.what()); + } + } + + return changed; +} + /// Populate a SoftJointLimits instance from the ROS parameter server. /** * It is assumed that the parameter structure is the following: @@ -550,6 +755,85 @@ inline bool get_joint_limits( lifecycle_node->get_node_logging_interface(), soft_limits); } +/** + * Check if any of updated parameters are related to SoftJointLimits. + * + * This method is intended to be used in the parameters update callback. + * It is recommended that it's result is temporarily stored and synchronized with the + * SoftJointLimits structure in the main loop. + * + * \param[in] joint_name Name of the joint whose limits should be checked. + * \param[in] parameters List of the parameters that should be checked if they belong to this + * structure and that are used for updating the internal values. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] updated_limits structure with updated SoftJointLimits. It is recommended that the + * currently used limits are stored into this structure before calling this method. + */ +inline bool check_for_limits_update( + const std::string & joint_name, const std::vector & parameters, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + SoftJointLimits & updated_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + bool changed = false; + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".has_soft_limits") + { + if (!parameter.get_value()) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "Parameter 'has_soft_limits' is not set, therefore the limits will not be updated!"); + return false; + } + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".k_position") + { + changed = updated_limits.k_position != parameter.get_value(); + updated_limits.k_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".k_velocity") + { + changed = updated_limits.k_velocity != parameter.get_value(); + updated_limits.k_velocity = parameter.get_value(); + } + else if (param_name == param_base_name + ".soft_lower_limit") + { + changed = updated_limits.min_position != parameter.get_value(); + updated_limits.min_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".soft_upper_limit") + { + changed = updated_limits.max_position != parameter.get_value(); + updated_limits.max_position = parameter.get_value(); + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + return changed; +} + } // namespace joint_limits #endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp new file mode 100644 index 0000000000..3641fc5c75 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2024, 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 Dr. Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ +#define JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ + +#include +#include +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" + +namespace joint_limits +{ +/** + * Joint Saturation Limiter limits joints' position, velocity and acceleration by clamping values + * to its minimal and maximal allowed values. Since the position, velocity and accelerations are + * variables in physical relation, it might be that some values are limited lower then specified + * limit. For example, if a joint is close to its position limit, velocity and acceleration will be + * reduced accordingly. + */ +template +class JointSaturationLimiter : public JointLimiterInterface +{ +public: + /** \brief Constructor */ + JOINT_LIMITS_PUBLIC JointSaturationLimiter(); + + /** \brief Destructor */ + JOINT_LIMITS_PUBLIC ~JointSaturationLimiter(); + + JOINT_LIMITS_PUBLIC bool on_init() override { return true; } + + JOINT_LIMITS_PUBLIC bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) override + { + return true; + } + + /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. + * Class implements this method accepting following data types: + * - trajectory_msgs::msg::JointTrajectoryPoint: limiting position, velocity and acceleration; + * + * Implementation of saturation approach for joints with position, velocity or acceleration limits + * and values. First, position limits are checked to adjust desired velocity accordingly, then + * velocity and finally acceleration. + * The method support partial existence of limits, e.g., missing position limits for continuous + * joins. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; + + /** \brief Enforce joint limits to desired arbitrary physical quantity. + * Additional, commonly used method for enforcing limits by clamping desired input value. + * The limit is defined in effort limits of the `joint::limits/JointLimit` structure. + * + * If `has_effort_limits` is set to false, the limits will be not enforced to a joint. + * + * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the + * limits. \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC bool on_enforce(std::vector & desired_joint_states) override; + +private: + rclcpp::Clock::SharedPtr clock_; +}; + +template +JointSaturationLimiter::JointSaturationLimiter() : JointLimiterInterface() +{ + clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); +} + +template +JointSaturationLimiter::~JointSaturationLimiter() +{ +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h new file mode 100644 index 0000000000..54edbc15f3 --- /dev/null +++ b/joint_limits/include/joint_limits/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_LIMITS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_BUILDING_DLL +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT +#else +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT +#endif +#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#else +#define JOINT_LIMITS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#endif +#define JOINT_LIMITS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml new file mode 100644 index 0000000000..a49d88fbc9 --- /dev/null +++ b/joint_limits/joint_limiters.xml @@ -0,0 +1,11 @@ + + + + + Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. + + + + diff --git a/joint_limits/package.xml b/joint_limits/package.xml index be120cded6..ea109d517a 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,10 +1,10 @@ joint_limits 4.12.0 - Interfaces for handling of joint limits for controllers or hardware. + Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar - Denis Štogl + Denis Štogl Apache License 2.0 @@ -15,13 +15,19 @@ ament_cmake ament_cmake_gen_version_h + backward_ros + pluginlib + realtime_tools rclcpp rclcpp_lifecycle + trajectory_msgs urdf + ament_cmake_gmock + ament_cmake_gtest + generate_parameter_library launch_ros launch_testing_ament_cmake - ament_cmake_gtest ament_cmake diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp new file mode 100644 index 0000000000..aea0e6704d --- /dev/null +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -0,0 +1,21 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Dr. Denis Stogl + +#include "joint_limits/joint_limiter_interface.hpp" + +namespace joint_limits +{ +} // namespace joint_limits diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp new file mode 100644 index 0000000000..2f8ac9636f --- /dev/null +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -0,0 +1,478 @@ +// Copyright (c) 2024, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \authors Nathan Brooks, Dr. Denis Stogl, Guillaume Walck + +#include "joint_limits/joint_saturation_limiter.hpp" + +#include + +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; + +namespace joint_limits +{ +template <> +bool JointSaturationLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) +{ + bool limits_enforced = false; + + const auto dt_seconds = dt.seconds(); + // negative or null is not allowed + if (dt_seconds <= 0.0) + { + return false; + } + + // TODO(gwalck) compute if the max are not implicitly violated with the given dt + // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4 + // velocity max is implicitly already violated due to max_acc * dt > 2.0 + + // check for required inputs combination + bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); + bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_); + bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_); + bool has_current_position = (current_joint_states.positions.size() == number_of_joints_); + bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_); + + // pos state and vel or pos cmd is required, vel state is optional + if (!(has_current_position && (has_desired_position || has_desired_velocity))) + { + return false; + } + + if (!has_current_velocity) + { + // First update() after activating does not have velocity available, assume 0 + current_joint_states.velocities.resize(number_of_joints_, 0.0); + } + + // TODO(destogl): please check if we get too much malloc from this initialization, + // if so then we should use members instead local variables and initialize them in other method + std::vector desired_pos(number_of_joints_); + std::vector desired_vel(number_of_joints_); + std::vector desired_acc(number_of_joints_); + std::vector expected_vel(number_of_joints_); + std::vector expected_pos(number_of_joints_); + + // limits triggered + std::vector limited_jnts_pos, limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; + + bool braking_near_position_limit_triggered = false; + + for (size_t index = 0; index < number_of_joints_; ++index) + { + if (has_desired_position) + { + desired_pos[index] = desired_joint_states.positions[index]; + } + if (has_desired_velocity) + { + desired_vel[index] = desired_joint_states.velocities[index]; + } + if (has_desired_acceleration) + { + desired_acc[index] = desired_joint_states.accelerations[index]; + } + + if (has_desired_position) + { + // limit position + if (joint_limits_[index].has_position_limits) + { + // clamp input pos_cmd + auto pos = std::clamp( + desired_pos[index], joint_limits_[index].min_position, joint_limits_[index].max_position); + if (pos != desired_pos[index]) + { + desired_pos[index] = pos; + limited_jnts_pos.emplace_back(joint_names_[index]); + limits_enforced = true; + } + } + // priority to pos_cmd derivative over cmd_vel when not defined. If done always then we might + // get jumps in the velocity based on the system's dynamics. Position limit clamping is done + // below once again. + const double position_difference = desired_pos[index] - current_joint_states.positions[index]; + if ( + std::fabs(position_difference) > VALUE_CONSIDERED_ZERO && + std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO) + { + desired_vel[index] = position_difference / dt_seconds; + } + } + + // limit velocity + if (joint_limits_[index].has_velocity_limits) + { + // if desired velocity is not defined calculate it from positions + if (std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_vel[index])) + { + desired_vel[index] = + (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; + } + // clamp input vel_cmd + if (std::fabs(desired_vel[index]) > joint_limits_[index].max_velocity) + { + desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); + limited_jnts_vel.emplace_back(joint_names_[index]); + limits_enforced = true; + + // recompute pos_cmd if needed + if (has_desired_position) + { + desired_pos[index] = + current_joint_states.positions[index] + desired_vel[index] * dt_seconds; + } + + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + } + } + + // limit acceleration + if ( + joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits) + { + // if(has_current_velocity) + if (1) // for now use a zero velocity if not provided + { + // limiting acc or dec function + auto apply_acc_or_dec_limit = [&]( + const double max_acc_or_dec, std::vector & acc, + std::vector & limited_jnts) -> bool + { + if (std::fabs(acc[index]) > max_acc_or_dec) + { + acc[index] = std::copysign(max_acc_or_dec, acc[index]); + limited_jnts.emplace_back(joint_names_[index]); + limits_enforced = true; + return true; + } + else + { + return false; + } + }; + + // if desired acceleration if not provided compute it from desired_vel and vel_state + if ( + std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index])) + { + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + } + + // check if decelerating - if velocity is changing toward 0 + bool deceleration_limit_applied = false; + bool limit_applied = false; + if ( + (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) || + (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0)) + { + // limit deceleration + if (joint_limits_[index].has_deceleration_limits) + { + limit_applied = apply_acc_or_dec_limit( + joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec); + deceleration_limit_applied = true; + } + } + + // limit acceleration (fallback to acceleration if no deceleration limits) + if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied) + { + limit_applied = apply_acc_or_dec_limit( + joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc); + } + + if (limit_applied) + { + // vel_cmd from integration of desired_acc, needed even if no vel output + desired_vel[index] = + current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + if (has_desired_position) + { + // pos_cmd from from double integration of desired_acc + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + } + } + } + // else we cannot compute acc, so not limiting it + } + + // plan ahead for position limits + if (joint_limits_[index].has_position_limits) + { + if (has_desired_velocity && !has_desired_position) + { + // Check immediate next step when using vel_cmd only, other cases already handled + // integrate pos + expected_pos[index] = + current_joint_states.positions[index] + desired_vel[index] * dt_seconds; + // if expected_pos over limit + auto pos = std::clamp( + expected_pos[index], joint_limits_[index].min_position, + joint_limits_[index].max_position); + if (pos != expected_pos[index]) + { + // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full + // deceleration in any case limit pos to max + expected_pos[index] = pos; + // and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be + // zero) + desired_vel[index] = + (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; + limited_jnts_pos.emplace_back(joint_names_[index]); + limits_enforced = true; + } + } + + // Check that stopping distance is within joint limits + // Slow down all joints at maximum decel if any don't have stopping distance within joint + // limits + + // delta_x = (v2*v2 - v1*v1) / (2*a) + // stopping_distance = (- v1*v1) / (2*max_acceleration) + // Here we assume we will not trigger velocity limits while maximally decelerating. + // This is a valid assumption if we are not currently at a velocity limit since we are just + // coming to a rest. + double stopping_deccel = std::fabs(desired_vel[index] / dt_seconds); + if (joint_limits_[index].has_deceleration_limits) + { + stopping_deccel = joint_limits_[index].max_deceleration; + } + else if (joint_limits_[index].has_acceleration_limits) + { + stopping_deccel = joint_limits_[index].max_acceleration; + } + + double stopping_distance = + std::fabs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel)); + // compute stopping duration at stopping_deccel + double stopping_duration = std::fabs((desired_vel[index]) / (stopping_deccel)); + + // Check that joint limits are beyond stopping_distance and desired_velocity is towards + // that limit + if ( + (desired_vel[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + stopping_distance)) || + (desired_vel[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + stopping_distance))) + { + limited_jnts_pos.emplace_back(joint_names_[index]); + braking_near_position_limit_triggered = true; + limits_enforced = true; + } + else + { + // compute the travel_distance at new desired velocity, with best case duration + // stopping_duration + double motion_after_stopping_duration = desired_vel[index] * stopping_duration; + // re-check what happens if we don't slow down + if ( + (desired_vel[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + motion_after_stopping_duration)) || + (desired_vel[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + motion_after_stopping_duration))) + { + limited_jnts_pos.emplace_back(joint_names_[index]); + braking_near_position_limit_triggered = true; + limits_enforced = true; + } + // else no need to slow down. in worse case we won't hit the limit at current velocity + } + } + } + + // update variables according to triggers + if (braking_near_position_limit_triggered) + { + // this limit applies to all joints even if a single one is triggered + for (size_t index = 0; index < number_of_joints_; ++index) + { + // Compute accel to stop + // Here we aren't explicitly maximally decelerating, but for joints near their limits this + // should still result in max decel being used + desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds; + if (joint_limits_[index].has_deceleration_limits) + { + desired_acc[index] = std::copysign( + std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_deceleration), + desired_acc[index]); + } + else if (joint_limits_[index].has_acceleration_limits) + { + desired_acc[index] = std::copysign( + std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_acceleration), + desired_acc[index]); + } + + // Recompute velocity and position + if (has_desired_velocity) + { + desired_vel[index] = + current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + } + if (has_desired_position) + { + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + } + } + std::ostringstream ostr; + for (auto jnt : limited_jnts_pos) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() + << "] would exceed position limits" + " if continuing at current state, limiting all joints"); + } + + // display limitations + + // if position limiting + if (limited_jnts_pos.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_pos) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed position limits, limiting"); + } + + if (limited_jnts_vel.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_vel) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting"); + } + + if (limited_jnts_acc.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_acc) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting"); + } + + if (limited_jnts_dec.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_dec) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed deceleration limits, limiting"); + } + + if (has_desired_position) + { + desired_joint_states.positions = desired_pos; + } + if (has_desired_velocity) + { + desired_joint_states.velocities = desired_vel; + } + if (has_desired_acceleration) + { + desired_joint_states.accelerations = desired_acc; + } + + return limits_enforced; +} + +template <> +bool JointSaturationLimiter::on_enforce(std::vector & desired_joint_states) +{ + std::vector limited_joints_effort; + bool limits_enforced = false; + + bool has_cmd = (desired_joint_states.size() == number_of_joints_); + if (!has_cmd) + { + return false; + } + + for (size_t index = 0; index < number_of_joints_; ++index) + { + if (joint_limits_[index].has_effort_limits) + { + double max_effort = joint_limits_[index].max_effort; + if (std::fabs(desired_joint_states[index]) > max_effort) + { + desired_joint_states[index] = std::copysign(max_effort, desired_joint_states[index]); + limited_joints_effort.emplace_back(joint_names_[index]); + limits_enforced = true; + } + } + } + + if (limited_joints_effort.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_joints_effort) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed effort limits, limiting"); + } + + return limits_enforced; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + joint_limits::JointSaturationLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits/test/joint_saturation_limiter_param.yaml b/joint_limits/test/joint_saturation_limiter_param.yaml new file mode 100644 index 0000000000..f025421519 --- /dev/null +++ b/joint_limits/test/joint_saturation_limiter_param.yaml @@ -0,0 +1,57 @@ +joint_saturation_limiter: + ros__parameters: + joint_limits: + # Get full specification from parameter server + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 7.5 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: true + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 + + foo_joint_no_effort: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 7.5 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: false + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 + +joint_saturation_limiter_nodeclimit: + ros__parameters: + joint_limits: + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp new file mode 100644 index 0000000000..6ecbd46d71 --- /dev/null +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -0,0 +1,569 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Dr. Denis Stogl, Guillaume Walck + +#include "test_joint_saturation_limiter.hpp" + +TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test JointSaturationLimiter loading + ASSERT_NO_THROW( + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); + ASSERT_NE(joint_limiter_, nullptr); +} + +// NOTE: We accept also if there is no limit defined for a joint. +TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + // initialize the limiter + std::vector joint_names = {"bar_joint"}; + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); + } +} + +TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + // test no desired interface + desired_joint_states_.positions.clear(); + desired_joint_states_.velocities.clear(); + // currently not handled desired_joint_states_.accelerations.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + joint_limiter_->configure(last_commanded_state_); + + rclcpp::Duration period(1, 0); // 1 second + // test no position interface + current_joint_states_.positions.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // also fail if out of limits + desired_joint_states_.positions[0] = 20.0; + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + joint_limiter_->configure(last_commanded_state_); + + rclcpp::Duration period(1, 0); // 1 second + // test no vel interface + current_joint_states_.velocities.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + // also fail if out of limits + desired_joint_states_.positions[0] = 20.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + + // within limits + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if no limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.0, // pos unchanged + 1.0, // vel unchanged + 1.0 // acc = vel / 1.0 + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_with_acc) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + + // within limits + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.5; // valid pos derivative as well + desired_joint_states_.accelerations[0] = 2.9; // valid pos derivative as well + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if no limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.0, // pos unchanged + 1.5, // vel unchanged + 2.9 // acc = vel / 1.0 + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // pos/vel cmd ifs + current_joint_states_.positions[0] = -2.0; + desired_joint_states_.positions[0] = 1.0; + // desired velocity exceeds although correctly computed from pos derivative + desired_joint_states_.velocities[0] = 3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.0, // pos = pos + max_vel * dt + 2.0, // vel limited to max_vel + 2.0 / 1.0 // acc set to vel change/DT + ); + + // check opposite velocity direction (sign copy) + current_joint_states_.positions[0] = 2.0; + desired_joint_states_.positions[0] = -1.0; + // desired velocity exceeds although correctly computed from pos derivative + desired_joint_states_.velocities[0] = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.0, // pos = pos - max_vel * dt + -2.0, // vel limited to -max_vel + -2.0 / 1.0 // acc set to vel change/DT + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = 1.9; + // desired pos leads to vel exceeded (4.0 / sec instead of 2.0) + desired_joint_states_.positions[0] = 4.0; + // no vel cmd (will lead to internal computation of velocity) + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_EQ(desired_joint_states_.positions[0], 2.0); // pos limited to max_vel * DT + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT + + // check opposite position and direction + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = -1.9; + desired_joint_states_.positions[0] = -4.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_EQ(desired_joint_states_.positions[0], -2.0); // pos limited to -max_vel * DT + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // vel cmd ifs + current_joint_states_.positions[0] = -2.0; + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = 1.9; + // no pos cmd + desired_joint_states_.positions.clear(); + // desired velocity exceeds + desired_joint_states_.velocities[0] = 3.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + ASSERT_EQ(desired_joint_states_.velocities[0], 2.0); // vel limited to max_vel + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT + + // check opposite velocity direction + current_joint_states_.positions[0] = 2.0; + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = -1.9; + // desired velocity exceeds + desired_joint_states_.velocities[0] = -3.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + ASSERT_EQ(desired_joint_states_.velocities[0], -2.0); // vel limited to -max_vel + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // desired pos exceeds + current_joint_states_.positions[0] = 5.0; + desired_joint_states_.positions[0] = 20.0; + // no velocity interface + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos limits applied + ASSERT_EQ( + desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit) + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // using 0.05 because 1.0 sec invalidates the "small dt integration" + rclcpp::Duration period(0, 50000000); // 0.05 second + + // close to pos limit should reduce velocity and stop + current_joint_states_.positions[0] = 4.851; + current_joint_states_.velocities[0] = 1.5; + desired_joint_states_.positions[0] = 4.926; + desired_joint_states_.velocities[0] = 1.5; + + // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) + std::vector expected_ret = {true, true, true, false}; + for (auto i = 0u; i < 4; ++i) + { + auto previous_vel_request = desired_joint_states_.velocities[0]; + // expect limits applied until the end stop + ASSERT_EQ( + joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period), + expected_ret[i]); + + ASSERT_LE( + desired_joint_states_.velocities[0], + previous_vel_request); // vel adapted to reach end-stop should be decreasing + // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit + // hence no max deceleration anymore... + ASSERT_LT( + desired_joint_states_.positions[0], + 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits + ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate + + Integrate(period.seconds()); + + ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit + } + } +} + +TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity + desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.11125, // pos = double integration from max acc with current state + 0.35, // vel limited to vel + max acc * dt + 5.0 // acc max acc + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions[0] = + 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_NEAR( + desired_joint_states_.positions[0], 0.111250, + COMMON_THRESHOLD); // pos = double integration from max acc with current state + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + } +} + +TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions.clear(); // = 0.125; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + // no pos cmd ifs + ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + } +} + +TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 0.3; + current_joint_states_.velocities[0] = 0.5; + desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity + desired_joint_states_.velocities[0] = 0.1; // leads to acc < -max dec + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.315625, // pos = double integration from max dec with current state + 0.125, // vel limited by vel - max dec * dt + -7.5 // acc limited by -max dec + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) +{ + SetupNode("joint_saturation_limiter_nodeclimit"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 1.0; + current_joint_states_.velocities[0] = 1.0; + desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity + desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.04375, // pos = double integration from max acc with current state + 0.75, // vel limited by vel-max acc * dt + -5.0 // acc limited to -max acc + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_there_are_effort_limits_expect_them_to_be_applyed) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // value not over limit + desired_joint_states_.effort[0] = 15.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + + // value over limit + desired_joint_states_.effort[0] = 21.0; + ASSERT_TRUE(joint_limiter_->enforce(desired_joint_states_.effort)); + ASSERT_EQ(desired_joint_states_.effort[0], 20.0); + } +} + +TEST_F(JointSaturationLimiterTest, when_there_are_no_effort_limits_expect_them_not_applyed) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init("foo_joint_no_effort"); + Configure(); + + // value not over limit + desired_joint_states_.effort[0] = 15.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + + // value over limit + desired_joint_states_.effort[0] = 21.0; + ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); + ASSERT_EQ(desired_joint_states_.effort[0], 21.0); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp new file mode 100644 index 0000000000..3da0706177 --- /dev/null +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -0,0 +1,107 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_JOINT_SATURATION_LIMITER_HPP_ +#define TEST_JOINT_SATURATION_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +const double COMMON_THRESHOLD = 0.0011; + +#define CHECK_STATE_SINGLE_JOINT(tested_traj_point, idx, expected_pos, expected_vel, expected_acc) \ + EXPECT_NEAR(tested_traj_point.positions[idx], expected_pos, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.velocities[idx], expected_vel, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.accelerations[idx], expected_acc, COMMON_THRESHOLD); + +using JointLimiter = joint_limits::JointLimiterInterface; + +class JointSaturationLimiterTest : public ::testing::Test +{ +public: + void SetUp() override + { + node_name_ = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + } + + void SetupNode(const std::string node_name = "") + { + if (!node_name.empty()) + { + node_name_ = node_name; + } + node_ = std::make_shared(node_name_); + } + + void Load() + { + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + } + + void Init(const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + joint_limiter_->init(joint_names_, node_); + num_joints_ = joint_names_.size(); + last_commanded_state_.positions.resize(num_joints_, 0.0); + last_commanded_state_.velocities.resize(num_joints_, 0.0); + last_commanded_state_.accelerations.resize(num_joints_, 0.0); + last_commanded_state_.effort.resize(num_joints_, 0.0); + desired_joint_states_ = last_commanded_state_; + current_joint_states_ = last_commanded_state_; + } + + void Configure() { joint_limiter_->configure(last_commanded_state_); } + + void Integrate(double dt) + { + current_joint_states_.positions[0] += desired_joint_states_.velocities[0] * dt + + 0.5 * desired_joint_states_.accelerations[0] * dt * dt; + current_joint_states_.velocities[0] += desired_joint_states_.accelerations[0] * dt; + } + + JointSaturationLimiterTest() + : joint_limiter_type_("joint_limits/JointSaturationLimiter"), + joint_limiter_loader_( + "joint_limits", "joint_limits::JointLimiterInterface") + { + } + + void TearDown() override { node_.reset(); } + +protected: + std::string node_name_; + rclcpp::Node::SharedPtr node_; + std::vector joint_names_; + size_t num_joints_; + std::unique_ptr joint_limiter_; + std::string joint_limiter_type_; + pluginlib::ClassLoader joint_limiter_loader_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + trajectory_msgs::msg::JointTrajectoryPoint desired_joint_states_; + trajectory_msgs::msg::JointTrajectoryPoint current_joint_states_; +}; + +#endif // TEST_JOINT_SATURATION_LIMITER_HPP_