From 53656e4636ba0472a5ee20d3a29f096ff50f6f5d Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Wed, 1 Jan 2025 10:21:08 +0100 Subject: [PATCH 1/6] Bump version of pre-commit hooks (#1973) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 75c5402ffe..feefcc317d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.19.0 + rev: v3.19.1 hooks: - id: pyupgrade args: [--py36-plus] @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.4 + rev: v19.1.5 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] From ecfdfc76c653bccb78d98e607ac315967454c69e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 1 Jan 2025 22:46:20 +0100 Subject: [PATCH 2/6] Remove visibility include from docs (#1975) --- hardware_interface/doc/writing_new_hardware_component.rst | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index 5452af6cda..9a7f2fe625 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -22,14 +22,12 @@ The following is a step-by-step guide to create source files, basic tests, and c After creating the package, you should have at least ``CMakeLists.txt`` and ``package.xml`` files in it. Create also ``include//`` and ``src`` folders if they do not already exist. In ``include//`` folder add ``.hpp`` and ``.cpp`` in the ``src`` folder. - Optionally add ``visibility_control.h`` with the definition of export rules for Windows. - You can copy this file from an existing controller package and change the name prefix to the ````. #. **Adding declarations into header file (.hpp)** 1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). - 2. Include ``"hardware_interface/$interface_type$_interface.hpp"`` and ``visibility_control.h`` if you are using one. + 2. Include ``"hardware_interface/$interface_type$_interface.hpp"``. ``$interface_type$`` can be ``Actuator``, ``Sensor`` or ``System`` depending on the type of hardware you are using. for more details about each type check :ref:`Hardware Components description `. 3. Define a unique namespace for your hardware_interface. This is usually the package name written in ``snake_case``. From d8a21c4e5effa2ac3b75a3dffbc6b48471e18a1f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 4 Jan 2025 14:59:54 +0100 Subject: [PATCH 3/6] Add joint limiter interface plugins to enforce limits defined in the URDF (#1526) --- joint_limits/CMakeLists.txt | 33 + .../include/joint_limits/data_structures.hpp | 66 ++ .../joint_limits/joint_limiter_interface.hpp | 153 ++- .../include/joint_limits/joint_limits.hpp | 4 +- .../joint_limits/joint_limits_helpers.hpp | 92 ++ .../joint_limits/joint_saturation_limiter.hpp | 36 +- .../joint_limits/joint_soft_limiter.hpp | 70 ++ joint_limits/joint_limiters.xml | 20 +- joint_limits/src/joint_limits_helpers.cpp | 173 +++ joint_limits/src/joint_range_limiter.cpp | 172 +++ joint_limits/src/joint_saturation_limiter.cpp | 55 +- joint_limits/src/joint_soft_limiter.cpp | 269 +++++ joint_limits/test/test_joint_limiter.hpp | 155 +++ .../test/test_joint_range_limiter.cpp | 677 +++++++++++ .../test/test_joint_saturation_limiter.cpp | 42 - .../test/test_joint_saturation_limiter.hpp | 8 +- joint_limits/test/test_joint_soft_limiter.cpp | 1041 +++++++++++++++++ 17 files changed, 2883 insertions(+), 183 deletions(-) create mode 100644 joint_limits/include/joint_limits/data_structures.hpp create mode 100644 joint_limits/include/joint_limits/joint_limits_helpers.hpp create mode 100644 joint_limits/include/joint_limits/joint_soft_limiter.hpp create mode 100644 joint_limits/src/joint_limits_helpers.cpp create mode 100644 joint_limits/src/joint_range_limiter.cpp create mode 100644 joint_limits/src/joint_soft_limiter.cpp create mode 100644 joint_limits/test/test_joint_limiter.hpp create mode 100644 joint_limits/test/test_joint_range_limiter.cpp create mode 100644 joint_limits/test/test_joint_soft_limiter.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 444842313e..5f874bb601 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -44,14 +44,28 @@ target_include_directories(joint_limiter_interface PUBLIC ) ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_library(joint_limits_helpers SHARED + src/joint_limits_helpers.cpp +) +target_compile_features(joint_limits_helpers PUBLIC cxx_std_17) +target_include_directories(joint_limits_helpers PUBLIC + $ + $ +) +ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_library(joint_saturation_limiter SHARED src/joint_saturation_limiter.cpp + src/joint_range_limiter.cpp + src/joint_soft_limiter.cpp ) target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) target_include_directories(joint_saturation_limiter PUBLIC $ $ ) +target_link_libraries(joint_saturation_limiter PUBLIC joint_limits_helpers) + ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) @@ -91,6 +105,24 @@ if(BUILD_TESTING) rclcpp ) + ament_add_gmock(test_joint_range_limiter test/test_joint_range_limiter.cpp) + target_include_directories(test_joint_range_limiter PRIVATE include) + target_link_libraries(test_joint_range_limiter joint_limiter_interface) + ament_target_dependencies( + test_joint_range_limiter + pluginlib + rclcpp + ) + + ament_add_gmock(test_joint_soft_limiter test/test_joint_soft_limiter.cpp) + target_include_directories(test_joint_soft_limiter PRIVATE include) + target_link_libraries(test_joint_soft_limiter joint_limiter_interface) + ament_target_dependencies( + test_joint_soft_limiter + pluginlib + rclcpp + ) + endif() install( @@ -101,6 +133,7 @@ install(TARGETS joint_limits joint_limiter_interface joint_saturation_limiter + joint_limits_helpers EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/joint_limits/include/joint_limits/data_structures.hpp b/joint_limits/include/joint_limits/data_structures.hpp new file mode 100644 index 0000000000..b673908e67 --- /dev/null +++ b/joint_limits/include/joint_limits/data_structures.hpp @@ -0,0 +1,66 @@ +// Copyright 2024 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 Sai Kishor Kothakota + +#ifndef JOINT_LIMITS__DATA_STRUCTURES_HPP_ +#define JOINT_LIMITS__DATA_STRUCTURES_HPP_ + +#include +#include +#include + +namespace joint_limits +{ + +struct JointControlInterfacesData +{ + std::string joint_name; + std::optional position = std::nullopt; + std::optional velocity = std::nullopt; + std::optional effort = std::nullopt; + std::optional acceleration = std::nullopt; + std::optional jerk = std::nullopt; + + bool has_data() const + { + return has_position() || has_velocity() || has_effort() || has_acceleration() || has_jerk(); + } + + bool has_position() const { return position.has_value(); } + + bool has_velocity() const { return velocity.has_value(); } + + bool has_effort() const { return effort.has_value(); } + + bool has_acceleration() const { return acceleration.has_value(); } + + bool has_jerk() const { return jerk.has_value(); } +}; + +struct JointInterfacesCommandLimiterData +{ + std::string joint_name; + JointControlInterfacesData actual; + JointControlInterfacesData command; + JointControlInterfacesData prev_command; + JointControlInterfacesData limited; + + bool has_actual_data() const { return actual.has_data(); } + + bool has_command_data() const { return command.has_data(); } +}; + +} // namespace joint_limits +#endif // JOINT_LIMITS__DATA_STRUCTURES_HPP_ diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index af054634fa..cbe42999ab 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -20,6 +20,7 @@ #include #include +#include "joint_limits/joint_limits.hpp" #include "joint_limits/joint_limits_rosparam.hpp" #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -28,9 +29,8 @@ namespace joint_limits { -using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint; -template +template class JointLimiterInterface { public: @@ -68,55 +68,58 @@ class JointLimiterInterface // 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 (has_parameter_interface()) { - 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])) + for (size_t i = 0; i < number_of_joints_; ++i) { - RCLCPP_ERROR( - node_logging_itf_->get_logger(), - "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str()); - result = false; - break; + 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()); } - 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_); + 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; + 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; + 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]); - } + 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!"); - } + if (changed) + { + updated_limits_.writeFromNonRT(updated_joint_limits); + RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!"); + } - return set_parameters_result; - }; + return set_parameters_result; + }; - parameter_callback_ = - node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback); + parameter_callback_ = + node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback); + } if (result) { @@ -128,6 +131,34 @@ class JointLimiterInterface return result; } + /** + * Wrapper init method that accepts the joint names and their limits directly + */ + virtual bool init( + const std::vector & joint_names, + const std::vector & joint_limits, + const std::vector & soft_joint_limits, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf) + { + number_of_joints_ = joint_names.size(); + joint_names_ = joint_names; + joint_limits_ = joint_limits; + soft_joint_limits_ = soft_joint_limits; + node_param_itf_ = param_itf; + node_logging_itf_ = logging_itf; + updated_limits_.writeFromNonRT(joint_limits_); + + if ((number_of_joints_ != joint_limits_.size()) && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Number of joint names and limits do not match: %zu != %zu", + number_of_joints_, joint_limits_.size()); + } + return (number_of_joints_ == joint_limits_.size()) && on_init(); + } + /** * Wrapper init method that accepts pointer to the Node. * For details see other init method. @@ -177,19 +208,6 @@ class JointLimiterInterface 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. - */ - 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. * @@ -219,27 +237,32 @@ class JointLimiterInterface JointLimitsStateDataType & current_joint_states, JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; - /** \brief Method is realized by an implementation. + /** \brief Checks if the logging interface is set. + * \returns true if the logging interface is available, otherwise false. * - * 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. + * \note this way of interfacing would be useful for instances where the logging interface is not + * available, for example in the ResourceManager or ResourceStorage classes. + */ + bool has_logging_interface() const { return node_logging_itf_ != nullptr; } + + /** \brief Checks if the parameter interface is set. + * \returns true if the parameter interface is available, otherwise false. * - * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. - * \returns true if limits are enforced, otherwise false. + * * \note this way of interfacing would be useful for instances where the logging interface is + * not available, for example in the ResourceManager or ResourceStorage classes. */ - virtual bool on_enforce(std::vector & desired_joint_states) = 0; + bool has_parameter_interface() const { return node_param_itf_ != nullptr; } size_t number_of_joints_; std::vector joint_names_; - std::vector joint_limits_; + std::vector joint_limits_; + std::vector soft_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_; + realtime_tools::RealtimeBuffer> updated_limits_; }; } // namespace joint_limits diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp index 809bfd777b..40b0a206f2 100644 --- a/joint_limits/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -67,7 +67,7 @@ struct JointLimits bool has_effort_limits; bool angle_wraparound; - std::string to_string() + std::string to_string() const { std::stringstream ss_output; @@ -124,7 +124,7 @@ struct SoftJointLimits double k_position; double k_velocity; - std::string to_string() + std::string to_string() const { std::stringstream ss_output; diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp new file mode 100644 index 0000000000..368f09002a --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -0,0 +1,92 @@ +// Copyright 2024 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 Adrià Roig Moreno + +#ifndef JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ + +#include +#include +#include +#include "joint_limits/joint_limits.hpp" + +namespace joint_limits +{ +namespace internal +{ +constexpr double POSITION_BOUNDS_TOLERANCE = 0.002; +} + +/** + * @brief Checks if a value is limited by the given limits. + * @param value The value to check. + * @param min The minimum limit. + * @param max The maximum limit. + * @return True if the value is limited, false otherwise. + */ +bool is_limited(double value, double min, double max); + +/** + * @brief Computes the position limits based on the velocity and acceleration limits. + * @param limits The joint limits. + * @param act_vel The actual velocity of the joint. + * @param prev_command_pos The previous commanded position of the joint. + * @param dt The time step. + * @return The position limits, first is the lower limit and second is the upper limit. + */ +std::pair compute_position_limits( + const joint_limits::JointLimits & limits, const std::optional & act_vel, + const std::optional & prev_command_pos, double dt); + +/** + * @brief Computes the velocity limits based on the position and acceleration limits. + * @param joint_name The name of the joint. + * @param limits The joint limits. + * @param act_pos The actual position of the joint. + * @param prev_command_vel The previous commanded velocity of the joint. + * @param dt The time step. + * @return The velocity limits, first is the lower limit and second is the upper limit. + */ +std::pair compute_velocity_limits( + const std::string & joint_name, const joint_limits::JointLimits & limits, + const double & desired_vel, const std::optional & act_pos, + const std::optional & prev_command_vel, double dt); + +/** + * @brief Computes the effort limits based on the position and velocity limits. + * @param limits The joint limits. + * @param act_pos The actual position of the joint. + * @param act_vel The actual velocity of the joint. + * @param dt The time step. + * @return The effort limits, first is the lower limit and second is the upper limit. + */ +std::pair compute_effort_limits( + const joint_limits::JointLimits & limits, const std::optional & act_pos, + const std::optional & act_vel, double /*dt*/); + +/** + * @brief Computes the acceleration limits based on the change in velocity and acceleration and + * deceleration limits. + * @param limits The joint limits. + * @param desired_acceleration The desired acceleration. + * @param actual_velocity The actual velocity of the joint. + * @return The acceleration limits, first is the lower limit and second is the upper limit. + */ +std::pair compute_acceleration_limits( + const JointLimits & limits, double desired_acceleration, std::optional actual_velocity); + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 4ebdfbc5ad..9af574078d 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -33,8 +33,8 @@ namespace joint_limits * limit. For example, if a joint is close to its position limit, velocity and acceleration will be * reduced accordingly. */ -template -class JointSaturationLimiter : public JointLimiterInterface +template +class JointSaturationLimiter : public JointLimiterInterface { public: /** \brief Constructor */ @@ -45,9 +45,9 @@ class JointSaturationLimiter : public JointLimiterInterface bool on_init() override { return true; } - bool on_configure( - const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) override + bool on_configure(const JointLimitsStateDataType & current_joint_states) override { + prev_command_ = current_joint_states; return true; } @@ -67,33 +67,23 @@ class JointSaturationLimiter : public JointLimiterInterface * \returns true if limits are enforced, otherwise false. */ bool on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, - const rclcpp::Duration & dt) override; + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & 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. - */ - bool on_enforce(std::vector & desired_joint_states) override; - -private: +protected: rclcpp::Clock::SharedPtr clock_; + JointLimitsStateDataType prev_command_; }; -template -JointSaturationLimiter::JointSaturationLimiter() : JointLimiterInterface() +template +JointSaturationLimiter::JointSaturationLimiter() +: JointLimiterInterface() { clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); } -template -JointSaturationLimiter::~JointSaturationLimiter() +template +JointSaturationLimiter::~JointSaturationLimiter() { } diff --git a/joint_limits/include/joint_limits/joint_soft_limiter.hpp b/joint_limits/include/joint_limits/joint_soft_limiter.hpp new file mode 100644 index 0000000000..ebc9d99934 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_soft_limiter.hpp @@ -0,0 +1,70 @@ +// Copyright 2024 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 Adrià Roig Moreno + +#ifndef JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ +#define JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ + +#include +#include "joint_limits/data_structures.hpp" +#include "joint_limits/joint_limits_helpers.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" + +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; + +namespace joint_limits +{ + +class JointSoftLimiter : public JointSaturationLimiter +{ +public: + bool on_init() override + { + const bool result = (number_of_joints_ == 1); + if (!result && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : " + "%zu", + number_of_joints_); + } + prev_command_ = JointControlInterfacesData(); + return result; + } + + bool on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const rclcpp::Duration & dt) override; + + bool has_soft_position_limits(const joint_limits::SoftJointLimits & soft_joint_limits) + { + return std::isfinite(soft_joint_limits.min_position) && + std::isfinite(soft_joint_limits.max_position) && + (soft_joint_limits.max_position - soft_joint_limits.min_position) > + VALUE_CONSIDERED_ZERO; + } + + bool has_soft_limits(const joint_limits::SoftJointLimits & soft_joint_limits) + { + return has_soft_position_limits(soft_joint_limits) && + std::isfinite(soft_joint_limits.k_position) && + std::abs(soft_joint_limits.k_position) > VALUE_CONSIDERED_ZERO; + } +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index a49d88fbc9..f2a4b8e20c 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -1,11 +1,25 @@ - + Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. + + + Simple joint range limiter using clamping approach with the parsed limits. + + + + + Simple joint range limiter performing clamping between the parsed soft limits and the parsed joint limits. + + diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp new file mode 100644 index 0000000000..4fee92948c --- /dev/null +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -0,0 +1,173 @@ +// Copyright 2024 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 Adrià Roig Moreno + +#include "joint_limits/joint_limits_helpers.hpp" +#include +#include +#include "rclcpp/logging.hpp" + +namespace joint_limits +{ +namespace internal +{ +/** + * @brief Check if the limits are in the correct order and swap them if they are not. + */ +void check_and_swap_limits(std::pair & limits) +{ + if (limits.first > limits.second) + { + std::swap(limits.first, limits.second); + } +} +} // namespace internal + +bool is_limited(double value, double min, double max) { return value < min || value > max; } + +std::pair compute_position_limits( + const joint_limits::JointLimits & limits, const std::optional & act_vel, + const std::optional & prev_command_pos, double dt) +{ + std::pair pos_limits({limits.min_position, limits.max_position}); + if (limits.has_velocity_limits) + { + const double act_vel_abs = act_vel.has_value() ? std::fabs(act_vel.value()) : 0.0; + const double delta_vel = limits.has_acceleration_limits + ? act_vel_abs + (limits.max_acceleration * dt) + : limits.max_velocity; + const double max_vel = std::min(limits.max_velocity, delta_vel); + const double delta_pos = max_vel * dt; + pos_limits.first = std::max(prev_command_pos.value() - delta_pos, pos_limits.first); + pos_limits.second = std::min(prev_command_pos.value() + delta_pos, pos_limits.second); + } + internal::check_and_swap_limits(pos_limits); + return pos_limits; +} + +std::pair compute_velocity_limits( + const std::string & joint_name, const joint_limits::JointLimits & limits, + const double & desired_vel, const std::optional & act_pos, + const std::optional & prev_command_vel, double dt) +{ + const double max_vel = + limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::infinity(); + std::pair vel_limits({-max_vel, max_vel}); + if (limits.has_position_limits && act_pos.has_value()) + { + const double actual_pos = act_pos.value(); + const double max_vel_with_pos_limits = (limits.max_position - actual_pos) / dt; + const double min_vel_with_pos_limits = (limits.min_position - actual_pos) / dt; + vel_limits.first = std::max(min_vel_with_pos_limits, vel_limits.first); + vel_limits.second = std::min(max_vel_with_pos_limits, vel_limits.second); + + if (actual_pos > limits.max_position || actual_pos < limits.min_position) + { + if ( + (actual_pos < (limits.max_position + internal::POSITION_BOUNDS_TOLERANCE) && + (actual_pos > limits.min_position) && desired_vel >= 0.0) || + (actual_pos > (limits.min_position - internal::POSITION_BOUNDS_TOLERANCE) && + (actual_pos < limits.max_position) && desired_vel <= 0.0)) + { + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger("joint_limiter_interface"), + prev_command_vel.has_value() && prev_command_vel.value() != 0.0, + "Joint position %.5f is out of bounds[%.5f, %.5f] for the joint and we want to move " + "further into bounds with vel %.5f: '%s'. Joint velocity limits will be " + "restrictred to zero.", + actual_pos, limits.min_position, limits.max_position, desired_vel, joint_name.c_str()); + vel_limits = {0.0, 0.0}; + } + // If the joint reports a position way out of bounds, then it would mean something is + // extremely wrong, so no velocity command should be allowed as it might damage the robot + else if ( + (actual_pos > (limits.max_position + internal::POSITION_BOUNDS_TOLERANCE)) || + (actual_pos < (limits.min_position - internal::POSITION_BOUNDS_TOLERANCE))) + { + RCLCPP_ERROR_ONCE( + rclcpp::get_logger("joint_limiter_interface"), + "Joint position is out of bounds for the joint : '%s'. Joint velocity limits will be " + "restricted to zero.", + joint_name.c_str()); + vel_limits = {0.0, 0.0}; + } + } + } + if (limits.has_acceleration_limits && prev_command_vel.has_value()) + { + const double delta_vel = limits.max_acceleration * dt; + vel_limits.first = std::max(prev_command_vel.value() - delta_vel, vel_limits.first); + vel_limits.second = std::min(prev_command_vel.value() + delta_vel, vel_limits.second); + } + internal::check_and_swap_limits(vel_limits); + return vel_limits; +} + +std::pair compute_effort_limits( + const joint_limits::JointLimits & limits, const std::optional & act_pos, + const std::optional & act_vel, double /*dt*/) +{ + const double max_effort = + limits.has_effort_limits ? limits.max_effort : std::numeric_limits::infinity(); + std::pair eff_limits({-max_effort, max_effort}); + if (limits.has_position_limits && act_pos.has_value() && act_vel.has_value()) + { + if ((act_pos.value() <= limits.min_position) && (act_vel.value() <= 0.0)) + { + eff_limits.first = 0.0; + } + else if ((act_pos.value() >= limits.max_position) && (act_vel.value() >= 0.0)) + { + eff_limits.second = 0.0; + } + } + if (limits.has_velocity_limits && act_vel.has_value()) + { + if (act_vel.value() < -limits.max_velocity) + { + eff_limits.first = 0.0; + } + else if (act_vel.value() > limits.max_velocity) + { + eff_limits.second = 0.0; + } + } + internal::check_and_swap_limits(eff_limits); + return eff_limits; +} + +std::pair compute_acceleration_limits( + const joint_limits::JointLimits & limits, double desired_acceleration, + std::optional actual_velocity) +{ + std::pair acc_or_dec_limits( + -std::numeric_limits::infinity(), std::numeric_limits::infinity()); + if ( + limits.has_deceleration_limits && + ((desired_acceleration < 0 && actual_velocity && actual_velocity.value() > 0) || + (desired_acceleration > 0 && actual_velocity && actual_velocity.value() < 0))) + { + acc_or_dec_limits.first = -limits.max_deceleration; + acc_or_dec_limits.second = limits.max_deceleration; + } + else if (limits.has_acceleration_limits) + { + acc_or_dec_limits.first = -limits.max_acceleration; + acc_or_dec_limits.second = limits.max_acceleration; + } + return acc_or_dec_limits; +} + +} // namespace joint_limits diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp new file mode 100644 index 0000000000..1db7b3ef9a --- /dev/null +++ b/joint_limits/src/joint_range_limiter.cpp @@ -0,0 +1,172 @@ +// Copyright 2024 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 Sai Kishor Kothakota + +#include "joint_limits/joint_saturation_limiter.hpp" + +#include +#include "joint_limits/data_structures.hpp" +#include "joint_limits/joint_limits_helpers.hpp" +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +namespace joint_limits +{ + +template <> +bool JointSaturationLimiter::on_init() +{ + const bool result = (number_of_joints_ == 1); + if (!result && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : " + "%zu", + number_of_joints_); + } + prev_command_ = JointControlInterfacesData(); + return result; +} + +template <> +bool JointSaturationLimiter::on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + 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; + } + + const auto joint_limits = joint_limits_[0]; + const std::string joint_name = joint_names_[0]; + // The following conditional filling is needed for cases of having certain information missing + if (!prev_command_.has_data()) + { + if (actual.has_position()) + { + prev_command_.position = actual.position; + } + else if (desired.has_position()) + { + prev_command_.position = desired.position; + } + if (actual.has_velocity()) + { + prev_command_.velocity = actual.velocity; + } + else if (desired.has_velocity()) + { + prev_command_.velocity = desired.velocity; + } + if (actual.has_effort()) + { + prev_command_.effort = actual.effort; + } + else if (desired.has_effort()) + { + prev_command_.effort = desired.effort; + } + if (actual.has_acceleration()) + { + prev_command_.acceleration = actual.acceleration; + } + else if (desired.has_acceleration()) + { + prev_command_.acceleration = desired.acceleration; + } + if (actual.has_jerk()) + { + prev_command_.jerk = actual.jerk; + } + else if (desired.has_jerk()) + { + prev_command_.jerk = desired.jerk; + } + if (actual.has_data()) + { + prev_command_.joint_name = actual.joint_name; + } + else if (desired.has_data()) + { + prev_command_.joint_name = desired.joint_name; + } + } + + if (desired.has_position()) + { + const auto limits = + compute_position_limits(joint_limits, actual.velocity, prev_command_.position, dt_seconds); + limits_enforced = is_limited(desired.position.value(), limits.first, limits.second); + desired.position = std::clamp(desired.position.value(), limits.first, limits.second); + } + + if (desired.has_velocity()) + { + const auto limits = compute_velocity_limits( + joint_name, joint_limits, desired.velocity.value(), actual.position, prev_command_.velocity, + dt_seconds); + limits_enforced = + is_limited(desired.velocity.value(), limits.first, limits.second) || limits_enforced; + desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); + } + + if (desired.has_effort()) + { + const auto limits = + compute_effort_limits(joint_limits, actual.position, actual.velocity, dt_seconds); + limits_enforced = + is_limited(desired.effort.value(), limits.first, limits.second) || limits_enforced; + desired.effort = std::clamp(desired.effort.value(), limits.first, limits.second); + } + + if (desired.has_acceleration()) + { + const auto limits = + compute_acceleration_limits(joint_limits, desired.acceleration.value(), actual.velocity); + limits_enforced = + is_limited(desired.acceleration.value(), limits.first, limits.second) || limits_enforced; + desired.acceleration = std::clamp(desired.acceleration.value(), limits.first, limits.second); + } + + if (desired.has_jerk()) + { + limits_enforced = + is_limited(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk) || + limits_enforced; + desired.jerk = std::clamp(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk); + } + + prev_command_ = desired; + + return limits_enforced; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +// typedefs are needed here to avoid issues with macro expansion. ref: +// https://stackoverflow.com/a/8942986 +typedef joint_limits::JointSaturationLimiter + JointInterfacesSaturationLimiter; +typedef joint_limits::JointLimiterInterface + JointInterfacesLimiterInterfaceBase; +PLUGINLIB_EXPORT_CLASS(JointInterfacesSaturationLimiter, JointInterfacesLimiterInterfaceBase) diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index f2a3adae1e..a9987c4634 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -15,6 +15,7 @@ /// \authors Nathan Brooks, Dr. Denis Stogl, Guillaume Walck #include "joint_limits/joint_saturation_limiter.hpp" +#include "joint_limits/joint_limits_helpers.hpp" #include @@ -26,7 +27,7 @@ constexpr double VALUE_CONSIDERED_ZERO = 1e-10; namespace joint_limits { template <> -bool JointSaturationLimiter::on_enforce( +bool JointSaturationLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { @@ -426,52 +427,16 @@ bool JointSaturationLimiter::on_enforce( 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" +// typedefs are needed here to avoid issues with macro expansion. ref: +// https://stackoverflow.com/a/8942986 +using int_map = std::map; +using JointTrajectoryPointSaturationLimiter = + joint_limits::JointSaturationLimiter; +using JointTrajectoryPointLimiterInterfaceBase = + joint_limits::JointLimiterInterface; PLUGINLIB_EXPORT_CLASS( - joint_limits::JointSaturationLimiter, - joint_limits::JointLimiterInterface) + JointTrajectoryPointSaturationLimiter, JointTrajectoryPointLimiterInterfaceBase) diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp new file mode 100644 index 0000000000..a3d0d81ee9 --- /dev/null +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -0,0 +1,269 @@ +// Copyright 2024 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 Adrià Roig Moreno +#include "joint_limits/joint_soft_limiter.hpp" + +namespace joint_limits +{ + +bool JointSoftLimiter::on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + 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; + } + + const auto hard_limits = joint_limits_[0]; + joint_limits::SoftJointLimits soft_joint_limits; + if (!soft_joint_limits_.empty()) + { + soft_joint_limits = soft_joint_limits_[0]; + } + + const std::string joint_name = joint_names_[0]; + + if (!prev_command_.has_data()) + { + if (actual.has_position()) + { + prev_command_.position = actual.position; + } + else if (desired.has_position()) + { + prev_command_.position = desired.position; + } + if (actual.has_velocity()) + { + prev_command_.velocity = actual.velocity; + } + else if (desired.has_velocity()) + { + prev_command_.velocity = desired.velocity; + } + if (actual.has_effort()) + { + prev_command_.effort = actual.effort; + } + else if (desired.has_effort()) + { + prev_command_.effort = desired.effort; + } + if (actual.has_acceleration()) + { + prev_command_.acceleration = actual.acceleration; + } + else if (desired.has_acceleration()) + { + prev_command_.acceleration = desired.acceleration; + } + if (actual.has_jerk()) + { + prev_command_.jerk = actual.jerk; + } + else if (desired.has_jerk()) + { + prev_command_.jerk = desired.jerk; + } + if (actual.has_data()) + { + prev_command_.joint_name = actual.joint_name; + } + else if (desired.has_data()) + { + prev_command_.joint_name = desired.joint_name; + } + } + + double soft_min_vel = -std::numeric_limits::infinity(); + double soft_max_vel = std::numeric_limits::infinity(); + double position = std::numeric_limits::infinity(); + + if (actual.has_position()) + { + position = actual.position.value(); + } + else if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) + { + position = prev_command_.position.value(); + } + + if (hard_limits.has_velocity_limits) + { + soft_min_vel = -hard_limits.max_velocity; + soft_max_vel = hard_limits.max_velocity; + + if ( + hard_limits.has_position_limits && has_soft_limits(soft_joint_limits) && + std::isfinite(position)) + { + soft_min_vel = std::clamp( + -soft_joint_limits.k_position * (position - soft_joint_limits.min_position), + -hard_limits.max_velocity, hard_limits.max_velocity); + + soft_max_vel = std::clamp( + -soft_joint_limits.k_position * (position - soft_joint_limits.max_position), + -hard_limits.max_velocity, hard_limits.max_velocity); + + if ( + (position < (hard_limits.min_position - internal::POSITION_BOUNDS_TOLERANCE)) || + (position > (hard_limits.max_position + internal::POSITION_BOUNDS_TOLERANCE))) + { + soft_min_vel = 0.0; + soft_max_vel = 0.0; + } + else if ( + (position < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) + { + const double soft_limit_reach_velocity = 1.0 * (M_PI / 180.0); + soft_min_vel = std::copysign(soft_limit_reach_velocity, soft_min_vel); + soft_max_vel = std::copysign(soft_limit_reach_velocity, soft_max_vel); + } + } + } + + if (desired.has_position()) + { + const auto position_limits = + compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); + + double pos_low = -std::numeric_limits::infinity(); + double pos_high = std::numeric_limits::infinity(); + + if (has_soft_position_limits(soft_joint_limits)) + { + pos_low = soft_joint_limits.min_position; + pos_high = soft_joint_limits.max_position; + } + + if (hard_limits.has_velocity_limits) + { + pos_low = std::clamp(position + soft_min_vel * dt_seconds, pos_low, pos_high); + pos_high = std::clamp(position + soft_max_vel * dt_seconds, pos_low, pos_high); + } + pos_low = std::max(pos_low, position_limits.first); + pos_high = std::min(pos_high, position_limits.second); + + limits_enforced = is_limited(desired.position.value(), pos_low, pos_high); + desired.position = std::clamp(desired.position.value(), pos_low, pos_high); + } + + if (desired.has_velocity()) + { + const auto velocity_limits = compute_velocity_limits( + joint_name, hard_limits, desired.velocity.value(), actual.position, prev_command_.velocity, + dt_seconds); + + if (hard_limits.has_acceleration_limits && actual.has_velocity()) + { + soft_min_vel = + std::max(actual.velocity.value() - hard_limits.max_acceleration * dt_seconds, soft_min_vel); + soft_max_vel = + std::min(actual.velocity.value() + hard_limits.max_acceleration * dt_seconds, soft_max_vel); + } + + soft_min_vel = std::max(soft_min_vel, velocity_limits.first); + soft_max_vel = std::min(soft_max_vel, velocity_limits.second); + + limits_enforced = + is_limited(desired.velocity.value(), soft_min_vel, soft_max_vel) || limits_enforced; + desired.velocity = std::clamp(desired.velocity.value(), soft_min_vel, soft_max_vel); + } + + if (desired.has_effort()) + { + const auto effort_limits = + compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds); + + double soft_min_eff = effort_limits.first; + double soft_max_eff = effort_limits.second; + + if ( + hard_limits.has_effort_limits && std::isfinite(soft_joint_limits.k_velocity) && + actual.has_velocity()) + { + soft_min_eff = std::clamp( + -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_min_vel), + -hard_limits.max_effort, hard_limits.max_effort); + + soft_max_eff = std::clamp( + -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_max_vel), + -hard_limits.max_effort, hard_limits.max_effort); + + soft_min_eff = std::max(soft_min_eff, effort_limits.first); + soft_max_eff = std::min(soft_max_eff, effort_limits.second); + } + + limits_enforced = + is_limited(desired.effort.value(), soft_min_eff, soft_max_eff) || limits_enforced; + desired.effort = std::clamp(desired.effort.value(), soft_min_eff, soft_max_eff); + } + + if (desired.has_acceleration()) + { + const auto limits = + compute_acceleration_limits(hard_limits, desired.acceleration.value(), actual.velocity); + limits_enforced = + is_limited(desired.acceleration.value(), limits.first, limits.second) || limits_enforced; + desired.acceleration = std::clamp(desired.acceleration.value(), limits.first, limits.second); + } + + if (desired.has_jerk()) + { + limits_enforced = + is_limited(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk) || + limits_enforced; + desired.jerk = std::clamp(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk); + } + + if (desired.has_position() && !std::isfinite(desired.position.value()) && actual.has_position()) + { + desired.position = actual.position; + limits_enforced = true; + } + if (desired.has_velocity() && !std::isfinite(desired.velocity.value())) + { + desired.velocity = 0.0; + limits_enforced = true; + } + if (desired.has_acceleration() && !std::isfinite(desired.acceleration.value())) + { + desired.acceleration = 0.0; + limits_enforced = true; + } + if (desired.has_jerk() && !std::isfinite(desired.jerk.value())) + { + desired.jerk = 0.0; + limits_enforced = true; + } + + prev_command_ = desired; + + return limits_enforced; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +typedef joint_limits::JointSoftLimiter JointInterfacesSoftLimiter; +typedef joint_limits::JointLimiterInterface + JointInterfacesLimiterInterfaceBase; +PLUGINLIB_EXPORT_CLASS(JointInterfacesSoftLimiter, JointInterfacesLimiterInterfaceBase) diff --git a/joint_limits/test/test_joint_limiter.hpp b/joint_limits/test/test_joint_limiter.hpp new file mode 100644 index 0000000000..9886598e0e --- /dev/null +++ b/joint_limits/test/test_joint_limiter.hpp @@ -0,0 +1,155 @@ +// Copyright (c) 2023, 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_LIMITER_HPP_ +#define TEST_JOINT_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/data_structures.hpp" +#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" + +const double COMMON_THRESHOLD = 1.0e-6; + +using JointLimiter = joint_limits::JointLimiterInterface; + +class JointLimiterTest : 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_); + } + + bool Load() + { + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + return joint_limiter_ != nullptr; + } + + bool Init(const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + num_joints_ = joint_names_.size(); + last_commanded_state_.joint_name = joint_name; + last_commanded_state_.position = 0.0; + last_commanded_state_.velocity = 0.0; + last_commanded_state_.acceleration = 0.0; + last_commanded_state_.effort = 0.0; + desired_state_ = last_commanded_state_; + actual_state_ = last_commanded_state_; + return joint_limiter_->init(joint_names_, node_); + } + + bool Init(const joint_limits::JointLimits & limits, const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + num_joints_ = joint_names_.size(); + last_commanded_state_.joint_name = joint_name; + last_commanded_state_.position = 0.0; + last_commanded_state_.velocity = 0.0; + last_commanded_state_.acceleration = 0.0; + last_commanded_state_.effort = 0.0; + desired_state_ = last_commanded_state_; + actual_state_ = last_commanded_state_; + return joint_limiter_->init( + joint_names_, {limits}, std::vector(), nullptr, + node_->get_node_logging_interface()); + } + + bool Configure() { return joint_limiter_->configure(last_commanded_state_); } + + void Integrate(double dt) + { + actual_state_.position = actual_state_.position.value() + desired_state_.velocity.value() * dt + + 0.5 * desired_state_.acceleration.value() * dt * dt; + actual_state_.velocity = + actual_state_.velocity.value() + desired_state_.acceleration.value() * dt; + } + + explicit JointLimiterTest(const std::string & joint_limiter_type) + : joint_limiter_type_(joint_limiter_type), + joint_limiter_loader_( + "joint_limits", + "joint_limits::JointLimiterInterface") + { + } + + virtual ~JointLimiterTest() {} + + 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_; + + joint_limits::JointControlInterfacesData last_commanded_state_; + joint_limits::JointControlInterfacesData desired_state_; + joint_limits::JointControlInterfacesData actual_state_; +}; + +class JointSaturationLimiterTest : public JointLimiterTest +{ +public: + JointSaturationLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSaturationLimiter") + { + } +}; + +class JointSoftLimiterTest : public JointLimiterTest +{ +public: + JointSoftLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") {} + + bool Init( + const joint_limits::JointLimits & limits, const joint_limits::SoftJointLimits & soft_limits, + const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + num_joints_ = joint_names_.size(); + last_commanded_state_.joint_name = joint_name; + last_commanded_state_.position = 0.0; + last_commanded_state_.velocity = 0.0; + last_commanded_state_.acceleration = 0.0; + last_commanded_state_.effort = 0.0; + desired_state_ = last_commanded_state_; + actual_state_ = last_commanded_state_; + return joint_limiter_->init( + joint_names_, {limits}, {soft_limits}, nullptr, node_->get_node_logging_interface()); + } +}; + +#endif // TEST_JOINT_LIMITER_HPP_ diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp new file mode 100644 index 0000000000..083646d84f --- /dev/null +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -0,0 +1,677 @@ +// Copyright (c) 2023, 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 Sai Kishor Kothakota + +#include +#include "joint_limits/joint_limits_helpers.hpp" +#include "test_joint_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"); + ASSERT_TRUE(Load()); + // 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"); + ASSERT_TRUE(Load()); + + ASSERT_TRUE(Init()); + ASSERT_TRUE(Configure()); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); +} + +TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -M_PI; + limits.max_position = M_PI; + ASSERT_TRUE(Init(limits)); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.position = 4.0; + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // For hard limits, if there is no actual state but the desired state is outside the limit, then + // saturate it to the limits + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + desired_state_.position = -5.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.min_position, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // If the desired state is within the limits, then no saturation is needed + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now add the velocity limits + limits.max_velocity = 1.0; + limits.has_velocity_limits = true; + ASSERT_TRUE(Init(limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Let's set the desired position greater than reachable with max velocity limit + desired_state_.position = 2.0; + // As per max velocity limit, it can only reach 1.0 in 1 second + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + + // As per max velocity limit, it can only reach 0.0 in 1 second + desired_state_.position = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + + // Now let's check the case where the actual position is at 2.0 and the desired position is -M_PI + // with max velocity limit of 1.0 + ASSERT_TRUE(Init(limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = -M_PI; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now test when there are no position limits, then the desired position is not saturated + limits = joint_limits::JointLimits(); + ASSERT_TRUE(Init(limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = 5.0 * M_PI; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 5.0 * M_PI, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); +} + +TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + ASSERT_TRUE(Init(limits)); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.velocity = 2.0; + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now as the position limits are already configure, set the actual position nearby the limits, + // then the max velocity needs to adapt wrt to the position limits + // It is saturated as the position reported is close to the position limits + auto test_limit_enforcing = [&]( + const std::optional & actual_position, + double desired_velocity, double expected_velocity, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + + ", desired velocity: " + std::to_string(desired_velocity) + ", expected velocity: " + + std::to_string(expected_velocity) + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + desired_state_.velocity = desired_velocity; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + // Test cases when there is no actual position + // For hard limits, if there is no actual state but the desired state is outside the limit, then + // saturate it to the limits + test_limit_enforcing(std::nullopt, 2.0, 1.0, true); + test_limit_enforcing(std::nullopt, 1.1, 1.0, true); + test_limit_enforcing(std::nullopt, -5.0, -1.0, true); + test_limit_enforcing(std::nullopt, -std::numeric_limits::infinity(), -1.0, true); + test_limit_enforcing(std::nullopt, std::numeric_limits::infinity(), 1.0, true); + test_limit_enforcing(std::nullopt, -3.212, -1.0, true); + test_limit_enforcing(std::nullopt, -0.8, -0.8, false); + test_limit_enforcing(std::nullopt, 0.8, 0.8, false); + test_limit_enforcing(std::nullopt, 0.12, 0.12, false); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // The cases where the actual position value exist + test_limit_enforcing(4.5, 5.0, 0.5, true); + test_limit_enforcing(4.8, 5.0, 0.2, true); + test_limit_enforcing(4.5, 0.3, 0.3, false); + test_limit_enforcing(4.5, 0.5, 0.5, false); + test_limit_enforcing(5.0, 0.9, 0.0, true); + // When the position is out of the limits, then the velocity is saturated to zero + test_limit_enforcing(6.0, 2.0, 0.0, true); + test_limit_enforcing(6.0, -2.0, 0.0, true); + test_limit_enforcing(4.0, 0.5, 0.5, false); + test_limit_enforcing(-4.8, -6.0, -0.2, true); + test_limit_enforcing(4.3, 5.0, 0.7, true); + test_limit_enforcing(-4.5, -5.0, -0.5, true); + test_limit_enforcing(-4.5, -0.2, -0.2, false); + test_limit_enforcing(-3.0, -5.0, -1.0, true); + test_limit_enforcing(-3.0, -1.0, -1.0, false); + test_limit_enforcing(-5.0, -3.0, 0.0, true); + test_limit_enforcing(-5.0, -1.0, 0.0, true); + // When the position is out of the limits, then the velocity is saturated to zero + test_limit_enforcing(-6.0, -1.0, 0.0, true); + test_limit_enforcing(-6.0, -2.0, 0.0, true); + test_limit_enforcing(-6.0, 1.0, 0.0, true); + // If the reported actual position is within the limits and the tolerance, then the velocity is + // allowed to move into the range, but not away from the range + test_limit_enforcing( + -5.0 - joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, -3.0, 0.0, true); + test_limit_enforcing( + -5.0 - joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, 1.0, 1.0, false); + test_limit_enforcing( + -5.0 - joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, 0.2, 0.2, false); + test_limit_enforcing( + -5.0 - joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, 2.0, 1.0, true); + + test_limit_enforcing( + 5.0 + joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, 3.0, 0.0, true); + test_limit_enforcing( + 5.0 + joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, -1.0, -1.0, false); + test_limit_enforcing( + 5.0 + joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, -0.2, -0.2, false); + test_limit_enforcing( + 5.0 + joint_limits::internal::POSITION_BOUNDS_TOLERANCE / 2.0, -2.0, -1.0, true); + + // Now remove the position limits and only test with acceleration limits + limits.has_position_limits = false; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits)); + // Now the velocity limits are now saturated by the acceleration limits so in succeeding call it + // will reach the desired if it is within the max velocity limits. Here, the order of the tests is + // important. + for (auto act_pos : + {std::optional(std::nullopt), std::optional(10.0), + std::optional(-10.0)}) + { + test_limit_enforcing(act_pos, 0.0, 0.0, false); // Helps to reset th prev_command internally + test_limit_enforcing(act_pos, 1.0, 0.5, true); + test_limit_enforcing(act_pos, 1.0, 1.0, false); + test_limit_enforcing(act_pos, -0.2, 0.5, true); + test_limit_enforcing(act_pos, -0.2, 0.0, true); + test_limit_enforcing(act_pos, -0.2, -0.2, false); + test_limit_enforcing(act_pos, -0.3, -0.3, false); + test_limit_enforcing(act_pos, -0.9, -0.8, true); + test_limit_enforcing(act_pos, -0.9, -0.9, false); + test_limit_enforcing(act_pos, -2.0, -1.0, true); + test_limit_enforcing(act_pos, 2.0, -0.5, true); + test_limit_enforcing(act_pos, 2.0, 0.0, true); + test_limit_enforcing(act_pos, 2.0, 0.5, true); + test_limit_enforcing(act_pos, 2.0, 1.0, true); + test_limit_enforcing(act_pos, 0.0, 0.5, true); + test_limit_enforcing(act_pos, 0.0, 0.0, false); + } + + // Now re-enable the position limits and test with acceleration limits + limits.has_position_limits = true; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits)); + // Now the velocity limits are now saturated by the acceleration limits so in succeeding call it + // will reach the desired if it is within the max velocity limits. Here, the order of the tests is + // important. + test_limit_enforcing(4.5, 0.0, 0.0, false); // Helps to reset th prev_command internally + test_limit_enforcing(4.5, 1.0, 0.5, true); + test_limit_enforcing(4.8, 1.0, 0.2, true); + test_limit_enforcing(4.8, -1.0, -0.3, true); + test_limit_enforcing(4.8, -1.0, -0.8, true); + test_limit_enforcing(4.8, -1.0, -1.0, false); + // here the velocity is saturated by the acceleration limits, if not then the velocity will be + // directly -0.2 + test_limit_enforcing(-4.8, -2.0, -0.5, true); + test_limit_enforcing(-4.8, -1.0, -0.2, true); + test_limit_enforcing(-4.3, -1.0, -0.7, true); + test_limit_enforcing(-4.3, 0.0, -0.2, true); + test_limit_enforcing(-4.3, 0.0, 0.0, false); + test_limit_enforcing(-6.0, 1.0, 0.0, true); + test_limit_enforcing(-6.0, -1.0, 0.0, true); + test_limit_enforcing(6.0, 1.0, 0.0, true); + test_limit_enforcing(6.0, -1.0, 0.0, true); +} + +TEST_F(JointSaturationLimiterTest, check_desired_effort_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + limits.has_effort_limits = true; + limits.max_effort = 200.0; + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.effort = 20.0; + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_TRUE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now as the position limits are already configure, set the actual position nearby the limits, + // then the max velocity needs to adapt wrt to the position limits + // It is saturated as the position reported is close to the position limits + auto test_limit_enforcing = [&]( + const std::optional & actual_position, + const std::optional & actual_velocity, + double desired_effort, double expected_effort, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + ", actual velocity: " + + std::to_string(act_vel) + ", desired effort: " + std::to_string(desired_effort) + + ", expected effort: " + std::to_string(expected_effort) + ", is clamped: " + + std::to_string(is_clamped) + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.effort = desired_effort; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_TRUE(desired_state_.has_effort()); + EXPECT_NEAR(desired_state_.effort.value(), expected_effort, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.4), + std::optional(-0.2)}) + { + test_limit_enforcing(act_pos, act_vel, 20.0, 20.0, false); + test_limit_enforcing(act_pos, act_vel, 200.0, 200.0, false); + test_limit_enforcing(act_pos, act_vel, 201.0, 200.0, true); + test_limit_enforcing(act_pos, act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_pos, act_vel, -20.0, -20.0, false); + test_limit_enforcing(act_pos, act_vel, -200.0, -200.0, false); + test_limit_enforcing(act_pos, act_vel, -201.0, -200.0, true); + } + } + + // The convention is that the positive velocity/position will result in positive effort + // Now the cases where the actual position or the actual velocity is out of the limits + test_limit_enforcing(5.0, 0.0, 20.0, 0.0, true); + test_limit_enforcing(5.0, 0.0, 400.0, 0.0, true); + test_limit_enforcing(6.0, 0.0, 400.0, 0.0, true); + test_limit_enforcing(5.0, 0.0, -20.0, -20.0, false); + test_limit_enforcing(5.0, 0.0, -400.0, -200.0, true); + test_limit_enforcing(6.0, 0.0, -400.0, -200.0, true); + + // At the limit, when trying to move away from the limit, it should allow + test_limit_enforcing(5.0, -0.2, 400.0, 200.0, true); + test_limit_enforcing(5.0, -0.2, -400.0, -200.0, true); + test_limit_enforcing(5.0, -0.2, 30.0, 30.0, false); + test_limit_enforcing(5.0, -0.2, -30.0, -30.0, false); + // For the positive velocity with limit, the effort is saturated + test_limit_enforcing(5.0, 0.2, 400.0, 0.0, true); + test_limit_enforcing(5.0, 0.2, 30.0, 0.0, true); + test_limit_enforcing(5.0, 0.2, -400.0, -200.0, true); + test_limit_enforcing(5.0, 0.2, -30.0, -30.0, false); + + test_limit_enforcing(-5.0, 0.2, 20.0, 20.0, false); + test_limit_enforcing(-5.0, 0.2, 400.0, 200.0, true); + test_limit_enforcing(-5.0, 0.2, -20.0, -20.0, false); + test_limit_enforcing(-5.0, 0.2, -400.0, -200.0, true); + // For the negative velocity with limit, the effort is saturated + test_limit_enforcing(-5.0, -0.2, -400.0, 0.0, true); + test_limit_enforcing(-5.0, -0.2, -30.0, 0.0, true); + test_limit_enforcing(-5.0, -0.2, 400.0, 200.0, true); + test_limit_enforcing(-5.0, -0.2, 30.0, 30.0, false); +} + +TEST_F(JointSaturationLimiterTest, check_desired_acceleration_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = [&]( + const std::optional & actual_velocity, double desired_accel, + double expected_accel, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual velocity: " + std::to_string(act_vel) + ", desired acceleration: " + + std::to_string(desired_accel) + ", expected acceleration: " + std::to_string(expected_accel) + + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.acceleration = desired_accel; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_TRUE(desired_state_.has_acceleration()); + EXPECT_NEAR(desired_state_.acceleration.value(), expected_accel, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + // Tests without applying deceleration limits + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.4), + std::optional(-0.2)}) + { + test_limit_enforcing(act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_vel, 0.5, 0.5, false); + test_limit_enforcing(act_vel, 0.6, 0.5, true); + test_limit_enforcing(act_vel, 1.5, 0.5, true); + test_limit_enforcing(act_vel, -0.5, -0.5, false); + test_limit_enforcing(act_vel, -0.6, -0.5, true); + test_limit_enforcing(act_vel, -1.5, -0.5, true); + } + + // Now let's test with applying deceleration limits + limits.has_deceleration_limits = true; + limits.max_deceleration = 0.25; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits)); + + // If you don't have the actual velocity, the deceleration limits are not applied + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + test_limit_enforcing(std::nullopt, 0.5, 0.5, false); + test_limit_enforcing(std::nullopt, 0.6, 0.5, true); + test_limit_enforcing(std::nullopt, 1.5, 0.5, true); + test_limit_enforcing(std::nullopt, -0.5, -0.5, false); + test_limit_enforcing(std::nullopt, -0.6, -0.5, true); + test_limit_enforcing(std::nullopt, -1.5, -0.5, true); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // Testing both positive and negative velocities and accelerations together without a proper + // deceleration + test_limit_enforcing(0.4, 0.2, 0.2, false); + test_limit_enforcing(0.4, 0.8, 0.5, true); + test_limit_enforcing(-0.4, -0.2, -0.2, false); + test_limit_enforcing(-0.4, -0.6, -0.5, true); + + // The deceleration limits are basically applied when the acceleration is positive and the + // velocity is negative and when the acceleration is negative and the velocity is positive + test_limit_enforcing(0.4, -0.1, -0.1, false); + test_limit_enforcing(0.4, -0.25, -0.25, false); + test_limit_enforcing(0.4, -0.6, -0.25, true); + test_limit_enforcing(0.4, -4.0, -0.25, true); + test_limit_enforcing(-0.4, 0.1, 0.1, false); + test_limit_enforcing(-0.4, 0.25, 0.25, false); + test_limit_enforcing(-0.4, 0.6, 0.25, true); + test_limit_enforcing(-0.4, 3.0, 0.25, true); +} + +TEST_F(JointSaturationLimiterTest, check_desired_jerk_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_jerk_limits = true; + limits.max_jerk = 0.5; + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = [&](double desired_jerk, double expected_jerk, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + SCOPED_TRACE( + "Testing for desired jerk : " + std::to_string(desired_jerk) + ", expected jerk: " + + std::to_string(expected_jerk) + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + desired_state_.jerk = desired_jerk; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_TRUE(desired_state_.has_jerk()); + EXPECT_NEAR(desired_state_.jerk.value(), expected_jerk, COMMON_THRESHOLD); + }; + + // Check with jerk limits + test_limit_enforcing(0.0, 0.0, false); + test_limit_enforcing(0.5, 0.5, false); + test_limit_enforcing(0.6, 0.5, true); + test_limit_enforcing(1.5, 0.5, true); + test_limit_enforcing(-0.5, -0.5, false); + test_limit_enforcing(-0.6, -0.5, true); + test_limit_enforcing(-1.5, -0.5, true); +} + +TEST_F(JointSaturationLimiterTest, check_all_desired_references_limiting) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + limits.has_deceleration_limits = true; + limits.max_deceleration = 0.25; + limits.has_jerk_limits = true; + limits.max_jerk = 2.0; + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = + [&]( + const std::optional & actual_position, const std::optional & actual_velocity, + double desired_position, double desired_velocity, double desired_acceleration, + double desired_jerk, double expected_position, double expected_velocity, + double expected_acceleration, double expected_jerk, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + ", actual velocity: " + + std::to_string(act_vel) + ", desired position: " + std::to_string(desired_position) + + ", desired velocity: " + std::to_string(desired_velocity) + ", desired acceleration: " + + std::to_string(desired_acceleration) + ", desired jerk: " + std::to_string(desired_jerk) + + ", expected position: " + std::to_string(expected_position) + + ", expected velocity: " + std::to_string(expected_velocity) + ", expected acceleration: " + + std::to_string(expected_acceleration) + ", expected jerk: " + std::to_string(expected_jerk) + + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.position = desired_position; + desired_state_.velocity = desired_velocity; + desired_state_.acceleration = desired_acceleration; + desired_state_.jerk = desired_jerk; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_position, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.acceleration.value(), expected_acceleration, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.jerk.value(), expected_jerk, COMMON_THRESHOLD); + }; + + // Test cases when there is no actual position and velocity + // Desired position and velocity affected due to the acceleration limits + test_limit_enforcing(std::nullopt, std::nullopt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 2.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 1.0, 0.5, 0.5, 3.0, 1.0, 0.5, 0.5, false); + + // Now enforce the limits with actual position and velocity + ASSERT_TRUE(Init(limits)); + // Desired position and velocity affected due to the acceleration limits + test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.8, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true); +} + +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.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp index 6ecbd46d71..c04e0602ba 100644 --- a/joint_limits/test/test_joint_saturation_limiter.cpp +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -517,48 +517,6 @@ TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_exp } } -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); diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp index 20097ae591..50b419d876 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -33,7 +33,8 @@ const double COMMON_THRESHOLD = 0.0011; 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; +using JointLimiter = + joint_limits::JointLimiterInterface; class JointSaturationLimiterTest : public ::testing::Test { @@ -81,9 +82,10 @@ class JointSaturationLimiterTest : public ::testing::Test } JointSaturationLimiterTest() - : joint_limiter_type_("joint_limits/JointSaturationLimiter"), + : joint_limiter_type_("joint_limits/JointTrajectoryPointSaturationLimiter"), joint_limiter_loader_( - "joint_limits", "joint_limits::JointLimiterInterface") + "joint_limits", + "joint_limits::JointLimiterInterface") { } diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp new file mode 100644 index 0000000000..7deb2796ea --- /dev/null +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -0,0 +1,1041 @@ +// Copyright (c) 2023, 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 Adrià Roig Moreno + +#include +#include +#include "test_joint_limiter.hpp" + +TEST_F(JointSoftLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test JointSoftLimiter 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(JointSoftLimiterTest, when_joint_not_found_expect_init_fail) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + // initialize the limiter + std::vector joint_names = {"bar_joint"}; + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); +} + +TEST_F(JointSoftLimiterTest, when_invalid_dt_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + ASSERT_TRUE(JointLimiterTest::Init()); + ASSERT_TRUE(Configure()); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); +} + +TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) +{ + SetupNode("soft_joint_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -M_PI; + limits.max_position = M_PI; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.position = 4.0; + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // For hard limits, if there is no actual state but the desired state is outside the limit, then + // saturate it to the limits + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + desired_state_.position = -5.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.min_position, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // If the desired state is within the limits, then no saturation is needed + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Check defining only max_position soft_limit (shouldn't consider it) + soft_limits.max_position = 3.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 4.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + // Check defining max_position soft_limit equal to min_position soft_limit (shouldn't consider it) + soft_limits.min_position = 3.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 4.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + // Check defining min_postion soft_limit greater than max_position soft_limit (shouldn't consider + // it) + soft_limits.min_position = 5.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 4.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + // Check defining correct limits (lower than hard_limits). It should clamp to soft limits. + soft_limits.min_position = -1.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 4.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), soft_limits.max_position, COMMON_THRESHOLD); + // If desired is inside the limits shouldn't clamp. + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + // Clamp to min_position soft_limits + desired_state_.position = -2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), soft_limits.min_position, COMMON_THRESHOLD); + + // Now add the velocity limits + soft_limits = joint_limits::SoftJointLimits(); + limits.max_velocity = 1.0; + limits.has_velocity_limits = true; + ASSERT_TRUE(Init(limits, soft_limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Let's set the desired position greater than reachable with max velocity limit + desired_state_.position = 2.0; + // As per max velocity limit, it can only reach 1.0 in 1 second + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + // As per max velocity limit, it can only reach 1.0 in 1 second + + // If soft position limits are less restrictive consider velocity_limits + soft_limits.max_position = 1.5; + soft_limits.min_position = -1.5; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + // If soft position limits are more restrictive consider soft position limits + soft_limits.max_position = 0.75; + soft_limits.min_position = -0.75; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), soft_limits.max_position, COMMON_THRESHOLD); + desired_state_.position = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + // As per max velocity limit, it can only reach -0.25 in 1 second + EXPECT_NEAR(desired_state_.position.value(), -0.25, COMMON_THRESHOLD); + + // Now let's check the case where the actual position is at 2.0 and the desired position is -M_PI + // with max velocity limit of 1.0 + soft_limits = joint_limits::SoftJointLimits(); + ASSERT_TRUE(Init(limits, soft_limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = -M_PI; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + soft_limits.min_position = 1.5; + soft_limits.max_position = 3.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = -M_PI; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), soft_limits.min_position, COMMON_THRESHOLD); + + // Check using k_position + soft_limits = joint_limits::SoftJointLimits(); + soft_limits.k_position = 1.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + actual_state_.position = 0.95; + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.95, COMMON_THRESHOLD); + actual_state_.position = 1.5; + desired_state_.position = 2.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 2.0, COMMON_THRESHOLD); + + soft_limits.k_position = 1.0; + soft_limits.max_position = 1.5; + soft_limits.min_position = -1.5; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + actual_state_.position = 0.45; + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.45, COMMON_THRESHOLD); + actual_state_.position = 0.95; + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.5, COMMON_THRESHOLD); + + // The case of no actual position feedback + soft_limits.k_position = 0.5; + soft_limits.max_position = 1.5; + soft_limits.min_position = -1.5; + ASSERT_TRUE(Init(limits, soft_limits)); + actual_state_.position = std::nullopt; + desired_state_.position = 0.2; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.2, COMMON_THRESHOLD); + auto prev_command_state = desired_state_; + while (desired_state_.position.value() < (soft_limits.max_position - COMMON_THRESHOLD)) + { + desired_state_.position = 2.0; + double expected_pos = + prev_command_state.position.value() + + (soft_limits.max_position - prev_command_state.position.value()) * soft_limits.k_position; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_pos, COMMON_THRESHOLD); + prev_command_state = desired_state_; + } + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.5, COMMON_THRESHOLD); + + // More generic test case + soft_limits.k_position = 0.5; + soft_limits.max_position = 2.0; + soft_limits.min_position = -2.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + actual_state_.position = 0.2; + while (actual_state_.position.value() < (desired_state_.position.value() - COMMON_THRESHOLD)) + { + desired_state_.position = 2.0; + double expected_pos = + actual_state_.position.value() + + (soft_limits.max_position - actual_state_.position.value()) * soft_limits.k_position; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_pos, COMMON_THRESHOLD); + actual_state_.position = expected_pos; + } + + // Now test when there are no position limits and soft limits, then the desired position is not + // saturated + limits = joint_limits::JointLimits(); + soft_limits = joint_limits::SoftJointLimits(); + ASSERT_TRUE(Init(limits, soft_limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = 5.0 * M_PI; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 5.0 * M_PI, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); +} + +TEST_F(JointSoftLimiterTest, check_desired_velocity_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.velocity = 2.0; + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now as the position limits are already configure, set the actual position nearby the limits, + // then the max velocity needs to adapt wrt to the position limits + // It is saturated as the position reported is close to the position limits + auto test_limit_enforcing = [&]( + const std::optional & actual_position, + double desired_velocity, double expected_velocity, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + + ", desired velocity: " + std::to_string(desired_velocity) + ", expected velocity: " + + std::to_string(expected_velocity) + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + desired_state_.velocity = desired_velocity; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + auto test_generic_cases = [&](const joint_limits::SoftJointLimits & soft_joint_limits) + { + ASSERT_TRUE(Init(limits, soft_joint_limits)); + + // Test cases when there is no actual position + // For hard limits, if there is no actual state but the desired state is outside the limit, then + // saturate it to the limits + test_limit_enforcing(std::nullopt, 2.0, 1.0, true); + test_limit_enforcing(std::nullopt, 1.1, 1.0, true); + test_limit_enforcing(std::nullopt, -5.0, -1.0, true); + test_limit_enforcing(std::nullopt, -std::numeric_limits::infinity(), -1.0, true); + test_limit_enforcing(std::nullopt, std::numeric_limits::infinity(), 1.0, true); + test_limit_enforcing(std::nullopt, -3.212, -1.0, true); + test_limit_enforcing(std::nullopt, -0.8, -0.8, false); + test_limit_enforcing(std::nullopt, 0.8, 0.8, false); + test_limit_enforcing(std::nullopt, 0.12, 0.12, false); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // The cases where the actual position value exist + test_limit_enforcing(4.5, 5.0, 0.5, true); + test_limit_enforcing(4.8, 5.0, 0.2, true); + test_limit_enforcing(4.5, 0.3, 0.3, false); + test_limit_enforcing(4.5, 0.5, 0.5, false); + test_limit_enforcing(5.0, 0.9, 0.0, true); + // When the position is out of the limits, then the velocity is saturated to zero + test_limit_enforcing(6.0, 2.0, 0.0, true); + test_limit_enforcing(6.0, -2.0, 0.0, true); + test_limit_enforcing(4.0, 0.5, 0.5, false); + test_limit_enforcing(-4.8, -6.0, -0.2, true); + test_limit_enforcing(4.3, 5.0, 0.7, true); + test_limit_enforcing(-4.5, -5.0, -0.5, true); + test_limit_enforcing(-4.5, -0.2, -0.2, false); + test_limit_enforcing(-3.0, -5.0, -1.0, true); + test_limit_enforcing(-3.0, -1.0, -1.0, false); + test_limit_enforcing(-5.0, -3.0, 0.0, true); + test_limit_enforcing(-5.0, -1.0, 0.0, true); + // When the position is out of the limits, then the velocity is saturated to zero + test_limit_enforcing(-6.0, -1.0, 0.0, true); + test_limit_enforcing(-6.0, -2.0, 0.0, true); + test_limit_enforcing(-6.0, 1.0, 0.0, true); + }; + + test_generic_cases(soft_limits); + soft_limits.k_position = 1.0; + test_generic_cases(soft_limits); + soft_limits.max_position = 10.0; + test_generic_cases(soft_limits); + soft_limits.min_position = -std::numeric_limits::infinity(); + test_generic_cases(soft_limits); + soft_limits.min_position = -10.0; + test_generic_cases(soft_limits); + + soft_limits.k_position = 1.0; + soft_limits.max_position = 4.0; + soft_limits.min_position = -2.0; + ASSERT_TRUE(Init(limits, soft_limits)); + test_limit_enforcing(std::nullopt, 2.0, 1.0, true); + test_limit_enforcing(std::nullopt, 1.1, 1.0, true); + test_limit_enforcing(std::nullopt, -5.0, -1.0, true); + test_limit_enforcing(std::nullopt, -std::numeric_limits::infinity(), -1.0, true); + test_limit_enforcing(std::nullopt, std::numeric_limits::infinity(), 1.0, true); + test_limit_enforcing(std::nullopt, -3.212, -1.0, true); + test_limit_enforcing(std::nullopt, -0.8, -0.8, false); + test_limit_enforcing(std::nullopt, 0.8, 0.8, false); + test_limit_enforcing(std::nullopt, 0.12, 0.12, false); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // The cases where the actual position value exist + test_limit_enforcing(3.0, 5.0, 1.0, true); + test_limit_enforcing(3.8, 5.0, 0.2, true); + test_limit_enforcing(3.5, 0.3, 0.3, false); + test_limit_enforcing(3.5, 0.5, 0.5, false); + test_limit_enforcing(3.8, 0.5, 0.2, true); + + test_limit_enforcing(4.0, 0.5, 0.0, true); + test_limit_enforcing(-2.0, -0.5, 0.0, true); + test_limit_enforcing(4.0, -0.5, -0.5, false); + test_limit_enforcing(-2.0, 0.5, 0.5, false); + test_limit_enforcing(4.0, -3.0, -1.0, true); + test_limit_enforcing(-2.0, 3.0, 1.0, true); + + test_limit_enforcing(4.8, 0.6, -M_PI / 180., true); + test_limit_enforcing(4.3, 5.0, -M_PI / 180., true); + test_limit_enforcing(-4.5, -0.2, M_PI / 180., true); + test_limit_enforcing(-3.0, -5.0, M_PI / 180., true); + + test_limit_enforcing(4.8, -0.6, -M_PI / 180., true); + test_limit_enforcing(4.3, -5.0, -M_PI / 180., true); + test_limit_enforcing(-4.5, 0.2, M_PI / 180., true); + test_limit_enforcing(-3.0, 5.0, M_PI / 180., true); + + test_limit_enforcing(6.0, 2.0, 0.0, true); + test_limit_enforcing(6.0, -2.0, 0.0, true); + test_limit_enforcing(-6.0, -2.0, 0.0, true); + test_limit_enforcing(-6.0, 2.0, 0.0, true); + + test_limit_enforcing(-5.0, -3.0, M_PI / 180., true); + test_limit_enforcing(-5.0, -1.0, M_PI / 180., true); + test_limit_enforcing(5.0, 3.0, -M_PI / 180., true); + test_limit_enforcing(5.0, 1.0, -M_PI / 180., true); + test_limit_enforcing(-5.0, 3.0, M_PI / 180., true); + test_limit_enforcing(-5.0, 1.0, M_PI / 180., true); + test_limit_enforcing(5.0, -3.0, -M_PI / 180., true); + test_limit_enforcing(5.0, -1.0, -M_PI / 180., true); + + // Now remove the position limits and only test with acceleration limits + soft_limits = joint_limits::SoftJointLimits(); + limits.has_position_limits = false; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits, soft_limits)); + // Now the velocity limits are now saturated by the acceleration limits so in succeeding call it + // will reach the desired if it is within the max velocity limits. Here, the order of the tests is + // important. + for (auto act_pos : + {std::optional(std::nullopt), std::optional(10.0), + std::optional(-10.0)}) + { + test_limit_enforcing(act_pos, 0.0, 0.0, false); // Helps to reset th prev_command internally + test_limit_enforcing(act_pos, 1.0, 0.5, true); + test_limit_enforcing(act_pos, 1.0, 1.0, false); + test_limit_enforcing(act_pos, -0.2, 0.5, true); + test_limit_enforcing(act_pos, -0.2, 0.0, true); + test_limit_enforcing(act_pos, -0.2, -0.2, false); + test_limit_enforcing(act_pos, -0.3, -0.3, false); + test_limit_enforcing(act_pos, -0.9, -0.8, true); + test_limit_enforcing(act_pos, -0.9, -0.9, false); + test_limit_enforcing(act_pos, -2.0, -1.0, true); + test_limit_enforcing(act_pos, 2.0, -0.5, true); + test_limit_enforcing(act_pos, 2.0, 0.0, true); + test_limit_enforcing(act_pos, 2.0, 0.5, true); + test_limit_enforcing(act_pos, 2.0, 1.0, true); + test_limit_enforcing(act_pos, 0.0, 0.5, true); + test_limit_enforcing(act_pos, 0.0, 0.0, false); + } + + // Now re-enable the position limits and test with acceleration limits + limits.has_position_limits = true; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits, soft_limits)); + // Now the velocity limits are now saturated by the acceleration limits so in succeeding call it + // will reach the desired if it is within the max velocity limits. Here, the order of the tests is + // important. + test_limit_enforcing(4.5, 0.0, 0.0, false); // Helps to reset th prev_command internally + test_limit_enforcing(4.5, 1.0, 0.5, true); + test_limit_enforcing(4.8, 1.0, 0.2, true); + test_limit_enforcing(4.8, -1.0, -0.3, true); + test_limit_enforcing(4.8, -1.0, -0.8, true); + test_limit_enforcing(4.8, -1.0, -1.0, false); + // here the velocity is saturated by the acceleration limits, if not then the velocity will be + // directly -0.2 + test_limit_enforcing(-4.8, -2.0, -0.5, true); + test_limit_enforcing(-4.8, -1.0, -0.2, true); + test_limit_enforcing(-4.3, -1.0, -0.7, true); + test_limit_enforcing(-4.3, 0.0, -0.2, true); + test_limit_enforcing(-4.3, 0.0, 0.0, false); + test_limit_enforcing(-6.0, 1.0, 0.0, true); + test_limit_enforcing(-6.0, -1.0, 0.0, true); + test_limit_enforcing(6.0, 1.0, 0.0, true); + test_limit_enforcing(6.0, -1.0, 0.0, true); +} + +TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + limits.has_effort_limits = true; + limits.max_effort = 200.0; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.effort = 20.0; + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_TRUE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now as the position limits are already configure, set the actual position nearby the limits, + // then the max velocity needs to adapt wrt to the position limits + // It is saturated as the position reported is close to the position limits + auto test_limit_enforcing = [&]( + const std::optional & actual_position, + const std::optional & actual_velocity, + double desired_effort, double expected_effort, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + ", actual velocity: " + + std::to_string(act_vel) + ", desired effort: " + std::to_string(desired_effort) + + ", expected effort: " + std::to_string(expected_effort) + ", is clamped: " + + std::to_string(is_clamped) + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.effort = desired_effort; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_TRUE(desired_state_.has_effort()); + EXPECT_NEAR(desired_state_.effort.value(), expected_effort, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.4), + std::optional(-0.2)}) + { + test_limit_enforcing(act_pos, act_vel, 20.0, 20.0, false); + test_limit_enforcing(act_pos, act_vel, 200.0, 200.0, false); + test_limit_enforcing(act_pos, act_vel, 201.0, 200.0, true); + test_limit_enforcing(act_pos, act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_pos, act_vel, -20.0, -20.0, false); + test_limit_enforcing(act_pos, act_vel, -200.0, -200.0, false); + test_limit_enforcing(act_pos, act_vel, -201.0, -200.0, true); + } + } + + // The convention is that the positive velocity/position will result in positive effort + // Now the cases where the actual position or the actual velocity is out of the limits + test_limit_enforcing(5.0, 0.0, 20.0, 0.0, true); + test_limit_enforcing(5.0, 0.0, 400.0, 0.0, true); + test_limit_enforcing(6.0, 0.0, 400.0, 0.0, true); + test_limit_enforcing(5.0, 0.0, -20.0, -20.0, false); + test_limit_enforcing(5.0, 0.0, -400.0, -200.0, true); + test_limit_enforcing(6.0, 0.0, -400.0, -200.0, true); + + // At the limit, when trying to move away from the limit, it should allow + test_limit_enforcing(5.0, -0.2, 400.0, 200.0, true); + test_limit_enforcing(5.0, -0.2, -400.0, -200.0, true); + test_limit_enforcing(5.0, -0.2, 30.0, 30.0, false); + test_limit_enforcing(5.0, -0.2, -30.0, -30.0, false); + // For the positive velocity with limit, the effort is saturated + test_limit_enforcing(5.0, 0.2, 400.0, 0.0, true); + test_limit_enforcing(5.0, 0.2, 30.0, 0.0, true); + test_limit_enforcing(5.0, 0.2, -400.0, -200.0, true); + test_limit_enforcing(5.0, 0.2, -30.0, -30.0, false); + + test_limit_enforcing(-5.0, 0.2, 20.0, 20.0, false); + test_limit_enforcing(-5.0, 0.2, 400.0, 200.0, true); + test_limit_enforcing(-5.0, 0.2, -20.0, -20.0, false); + test_limit_enforcing(-5.0, 0.2, -400.0, -200.0, true); + // For the negative velocity with limit, the effort is saturated + test_limit_enforcing(-5.0, -0.2, -400.0, 0.0, true); + test_limit_enforcing(-5.0, -0.2, -30.0, 0.0, true); + test_limit_enforcing(-5.0, -0.2, 400.0, 200.0, true); + test_limit_enforcing(-5.0, -0.2, 30.0, 30.0, false); + + // Check with high values of k_velocity (hard limits should be considered) + soft_limits.k_velocity = 5000.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.5), + std::optional(-0.2)}) + { + test_limit_enforcing(act_pos, act_vel, 20.0, 20.0, false); + test_limit_enforcing(act_pos, act_vel, 200.0, 200.0, false); + test_limit_enforcing(act_pos, act_vel, 201.0, 200.0, true); + test_limit_enforcing(act_pos, act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_pos, act_vel, -20.0, -20.0, false); + test_limit_enforcing(act_pos, act_vel, -200.0, -200.0, false); + test_limit_enforcing(act_pos, act_vel, -201.0, -200.0, true); + } + } + + // Check with low values of k_velocity + soft_limits.k_velocity = 300.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + test_limit_enforcing(act_pos, 0.5, 20.0, 20.0, false); + test_limit_enforcing(act_pos, 0.5, 200.0, 150.0, true); + test_limit_enforcing(act_pos, 0.5, 201.0, 150.0, true); + test_limit_enforcing(act_pos, 0.5, 0.0, 0.0, false); + test_limit_enforcing(act_pos, 0.5, -20.0, -20.0, false); + test_limit_enforcing(act_pos, 0.5, -200.0, -200.0, false); + test_limit_enforcing(act_pos, 0.5, -201.0, -200.0, true); + test_limit_enforcing(act_pos, -0.5, 20.0, 20.0, false); + test_limit_enforcing(act_pos, -0.5, 200.0, 200.0, false); + test_limit_enforcing(act_pos, -0.5, 201.0, 200.0, true); + test_limit_enforcing(act_pos, -0.5, 0.0, 0.0, false); + test_limit_enforcing(act_pos, -0.5, -20.0, -20.0, false); + test_limit_enforcing(act_pos, -0.5, -200.0, -150.0, true); + test_limit_enforcing(act_pos, -0.5, -201.0, -150.0, true); + } + + // Check with low values of k_velocity and low soft_limits + soft_limits.k_velocity = 300.0; + soft_limits.k_position = 1.0; + soft_limits.min_position = -4.0; + soft_limits.max_position = 4.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + test_limit_enforcing(0.0, 0.5, 20.0, 20.0, false); + test_limit_enforcing(0.0, 0.5, 200.0, 150.0, true); + test_limit_enforcing(0.0, 0.5, 201.0, 150.0, true); + test_limit_enforcing(0.0, 0.5, 0.0, 0.0, false); + test_limit_enforcing(0.0, 0.5, -20.0, -20.0, false); + test_limit_enforcing(0.0, 0.5, -200.0, -200.0, false); + test_limit_enforcing(0.0, 0.5, -201.0, -200.0, true); + + test_limit_enforcing(0.0, -0.5, 20.0, 20.0, false); + test_limit_enforcing(0.0, -0.5, 200.0, 200.0, false); + test_limit_enforcing(0.0, -0.5, 201.0, 200.0, true); + test_limit_enforcing(0.0, -0.5, 0.0, 0.0, false); + test_limit_enforcing(0.0, -0.5, -20.0, -20.0, false); + test_limit_enforcing(0.0, -0.5, -200.0, -150.0, true); + test_limit_enforcing(0.0, -0.5, -201.0, -150.0, true); + + // Close to the soft limit with a velocity moving away from the limit + test_limit_enforcing(-3.5, 0.5, 20.0, 20.0, false); + test_limit_enforcing(-3.5, 0.5, 200.0, 150.0, true); + test_limit_enforcing(-3.5, 0.5, 201.0, 150.0, true); + test_limit_enforcing(-3.5, 0.5, 0.0, 0.0, false); + test_limit_enforcing(-3.5, 0.5, -20.0, -20.0, false); + test_limit_enforcing(-3.5, 0.5, -200.0, -200.0, false); + test_limit_enforcing(-3.5, 0.5, -201.0, -200.0, true); + + test_limit_enforcing(3.5, -0.5, 20.0, 20.0, false); + test_limit_enforcing(3.5, -0.5, 200.0, 200.0, false); + test_limit_enforcing(3.5, -0.5, 201.0, 200.0, true); + test_limit_enforcing(3.5, -0.5, 0.0, 0.0, false); + test_limit_enforcing(3.5, -0.5, -20.0, -20.0, false); + test_limit_enforcing(3.5, -0.5, -200.0, -150.0, true); + test_limit_enforcing(3.5, -0.5, -201.0, -150.0, true); + + // Close to the soft limit with a velocity moving close to the limit + test_limit_enforcing(3.5, 0.4, 20.0, 20.0, false); + test_limit_enforcing(3.5, 0.4, 200.0, 30.0, true); + test_limit_enforcing(3.5, 0.5, 200.0, 0.0, true); + test_limit_enforcing(3.5, 0.4, 201.0, 30.0, true); + test_limit_enforcing(3.5, 0.4, 0.0, 0.0, false); + test_limit_enforcing(3.5, 0.4, -20.0, -20.0, false); + test_limit_enforcing(3.5, 0.4, -200.0, -200.0, false); + test_limit_enforcing(3.5, 0.4, -201.0, -200.0, true); + + test_limit_enforcing(-3.5, -0.4, 20.0, 20.0, false); + test_limit_enforcing(-3.5, -0.4, 200.0, 200.0, false); + test_limit_enforcing(-3.5, -0.4, 201.0, 200.0, true); + test_limit_enforcing(-3.5, -0.4, 0.0, 0.0, false); + test_limit_enforcing(-3.5, -0.4, -20.0, -20.0, false); + test_limit_enforcing(-3.5, -0.4, -200.0, -30.0, true); + test_limit_enforcing(-3.5, -0.5, -200.0, 0.0, true); + test_limit_enforcing(-3.5, -0.4, -201.0, -30.0, true); + + // Check with high values of k_velocity and high soft limits (higher than hard limits) + soft_limits.k_velocity = 500.0; + soft_limits.k_position = 1.0; + soft_limits.min_position = -10.0; + soft_limits.max_position = 10.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.5), + std::optional(-0.2)}) + { + test_limit_enforcing(act_pos, act_vel, 20.0, 20.0, false); + test_limit_enforcing(act_pos, act_vel, 200.0, 200.0, false); + test_limit_enforcing(act_pos, act_vel, 201.0, 200.0, true); + test_limit_enforcing(act_pos, act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_pos, act_vel, -20.0, -20.0, false); + test_limit_enforcing(act_pos, act_vel, -200.0, -200.0, false); + test_limit_enforcing(act_pos, act_vel, -201.0, -200.0, true); + } + } + + // Check with low values of k_velocity and high soft limits (higher than hard limits) + soft_limits.k_velocity = 300.0; + soft_limits.k_position = 1.0; + soft_limits.min_position = -10.0; + soft_limits.max_position = 10.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + test_limit_enforcing(act_pos, 0.5, 20.0, 20.0, false); + test_limit_enforcing(act_pos, 0.5, 200.0, 150.0, true); + test_limit_enforcing(act_pos, 0.5, 201.0, 150.0, true); + test_limit_enforcing(act_pos, 0.5, 0.0, 0.0, false); + test_limit_enforcing(act_pos, 0.5, -20.0, -20.0, false); + test_limit_enforcing(act_pos, 0.5, -200.0, -200.0, false); + test_limit_enforcing(act_pos, 0.5, -201.0, -200.0, true); + + test_limit_enforcing(act_pos, -0.5, 20.0, 20.0, false); + test_limit_enforcing(act_pos, -0.5, 200.0, 200.0, false); + test_limit_enforcing(act_pos, -0.5, 201.0, 200.0, true); + test_limit_enforcing(act_pos, -0.5, 0.0, 0.0, false); + test_limit_enforcing(act_pos, -0.5, -20.0, -20.0, false); + test_limit_enforcing(act_pos, -0.5, -200.0, -150.0, true); + test_limit_enforcing(act_pos, -0.5, -201.0, -150.0, true); + } +} + +TEST_F(JointSoftLimiterTest, check_desired_acceleration_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = [&]( + const std::optional & actual_velocity, double desired_accel, + double expected_accel, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual velocity: " + std::to_string(act_vel) + ", desired acceleration: " + + std::to_string(desired_accel) + ", expected acceleration: " + std::to_string(expected_accel) + + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.acceleration = desired_accel; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_TRUE(desired_state_.has_acceleration()); + EXPECT_NEAR(desired_state_.acceleration.value(), expected_accel, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + // Tests without applying deceleration limits + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.4), + std::optional(-0.2)}) + { + test_limit_enforcing(act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_vel, 0.5, 0.5, false); + test_limit_enforcing(act_vel, 0.6, 0.5, true); + test_limit_enforcing(act_vel, 1.5, 0.5, true); + test_limit_enforcing(act_vel, -0.5, -0.5, false); + test_limit_enforcing(act_vel, -0.6, -0.5, true); + test_limit_enforcing(act_vel, -1.5, -0.5, true); + } + + // Now let's test with applying deceleration limits + limits.has_deceleration_limits = true; + limits.max_deceleration = 0.25; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits, soft_limits)); + + // If you don't have the actual velocity, the deceleration limits are not applied + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + test_limit_enforcing(std::nullopt, 0.5, 0.5, false); + test_limit_enforcing(std::nullopt, 0.6, 0.5, true); + test_limit_enforcing(std::nullopt, 1.5, 0.5, true); + test_limit_enforcing(std::nullopt, -0.5, -0.5, false); + test_limit_enforcing(std::nullopt, -0.6, -0.5, true); + test_limit_enforcing(std::nullopt, -1.5, -0.5, true); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // Testing both positive and negative velocities and accelerations together without a proper + // deceleration + test_limit_enforcing(0.4, 0.2, 0.2, false); + test_limit_enforcing(0.4, 0.8, 0.5, true); + test_limit_enforcing(-0.4, -0.2, -0.2, false); + test_limit_enforcing(-0.4, -0.6, -0.5, true); + + // The deceleration limits are basically applied when the acceleration is positive and the + // velocity is negative and when the acceleration is negative and the velocity is positive + test_limit_enforcing(0.4, -0.1, -0.1, false); + test_limit_enforcing(0.4, -0.25, -0.25, false); + test_limit_enforcing(0.4, -0.6, -0.25, true); + test_limit_enforcing(0.4, -4.0, -0.25, true); + test_limit_enforcing(-0.4, 0.1, 0.1, false); + test_limit_enforcing(-0.4, 0.25, 0.25, false); + test_limit_enforcing(-0.4, 0.6, 0.25, true); + test_limit_enforcing(-0.4, 3.0, 0.25, true); +} + +TEST_F(JointSoftLimiterTest, check_desired_jerk_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_jerk_limits = true; + limits.max_jerk = 0.5; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = [&](double desired_jerk, double expected_jerk, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + SCOPED_TRACE( + "Testing for desired jerk : " + std::to_string(desired_jerk) + ", expected jerk: " + + std::to_string(expected_jerk) + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + desired_state_.jerk = desired_jerk; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_TRUE(desired_state_.has_jerk()); + EXPECT_NEAR(desired_state_.jerk.value(), expected_jerk, COMMON_THRESHOLD); + }; + + // Check with jerk limits + test_limit_enforcing(0.0, 0.0, false); + test_limit_enforcing(0.5, 0.5, false); + test_limit_enforcing(0.6, 0.5, true); + test_limit_enforcing(1.5, 0.5, true); + test_limit_enforcing(-0.5, -0.5, false); + test_limit_enforcing(-0.6, -0.5, true); + test_limit_enforcing(-1.5, -0.5, true); +} + +TEST_F(JointSoftLimiterTest, check_all_desired_references_limiting) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + limits.has_deceleration_limits = true; + limits.max_deceleration = 0.25; + limits.has_jerk_limits = true; + limits.max_jerk = 2.0; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = + [&]( + const std::optional & actual_position, const std::optional & actual_velocity, + double desired_position, double desired_velocity, double desired_acceleration, + double desired_jerk, double expected_position, double expected_velocity, + double expected_acceleration, double expected_jerk, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + ", actual velocity: " + + std::to_string(act_vel) + ", desired position: " + std::to_string(desired_position) + + ", desired velocity: " + std::to_string(desired_velocity) + ", desired acceleration: " + + std::to_string(desired_acceleration) + ", desired jerk: " + std::to_string(desired_jerk) + + ", expected position: " + std::to_string(expected_position) + + ", expected velocity: " + std::to_string(expected_velocity) + ", expected acceleration: " + + std::to_string(expected_acceleration) + ", expected jerk: " + std::to_string(expected_jerk) + + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.position = desired_position; + desired_state_.velocity = desired_velocity; + desired_state_.acceleration = desired_acceleration; + desired_state_.jerk = desired_jerk; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_position, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.acceleration.value(), expected_acceleration, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.jerk.value(), expected_jerk, COMMON_THRESHOLD); + }; + + // Test cases when there is no actual position and velocity + // Desired position and velocity affected due to the acceleration limits + test_limit_enforcing(std::nullopt, std::nullopt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 2.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 1.0, 0.5, 0.5, 3.0, 1.0, 0.5, 0.5, false); + + // Now enforce the limits with actual position and velocity + ASSERT_TRUE(Init(limits, soft_limits)); + // Desired position and velocity affected due to the acceleration limits + test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.8, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 6e1059c9dfe3312f5b4d2f45c9d3d19d59685ee5 Mon Sep 17 00:00:00 2001 From: verma nakul Date: Tue, 7 Jan 2025 12:11:22 +0530 Subject: [PATCH 4/6] Using urdf/model.hpp for rolling (#1978) --- hardware_interface/src/component_parser.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index a3e9efaa3a..e1ff42ee14 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -20,7 +20,12 @@ #include #include +#include "rclcpp/version.h" +#if RCLCPP_VERSION_GTE(29, 0, 0) +#include "urdf/model.hpp" +#else #include "urdf/model.h" +#endif #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" From 940cf5318413bebcfef4e8fe7560bb6d134f849d Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Wed, 8 Jan 2025 12:25:16 +0100 Subject: [PATCH 5/6] Initialize robot description in ControllerManager (#1983) --- controller_manager/src/controller_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 34566f95b3..fd59c89ea8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -255,7 +255,8 @@ ControllerManager::ControllerManager( chainable_loader_( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), - cm_node_options_(options) + cm_node_options_(options), + robot_description_(urdf) { initialize_parameters(); resource_manager_ = std::make_unique( From b32967913da494d08ce5cbaa3f60b4b43dc47fed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 9 Jan 2025 09:05:40 +0100 Subject: [PATCH 6/6] Trigger shutdown transition in destructor (#1979) --- .../controller_interface_base.hpp | 2 +- .../src/controller_interface_base.cpp | 25 +++++++++++++++++++ .../test/test_controller_interface.cpp | 13 ++++++++++ .../test/test_controller_with_options.cpp | 19 ++++++++++---- .../test/joint_limits_rosparam_test.cpp | 13 +++++++++- 5 files changed, 65 insertions(+), 7 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9dee4528a3..a548cd5856 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -99,7 +99,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy public: ControllerInterfaceBase() = default; - virtual ~ControllerInterfaceBase() = default; + virtual ~ControllerInterfaceBase(); /// Get configuration for controller's required command interfaces. /** diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 03d45fa384..b8dd9f770d 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -22,6 +22,22 @@ namespace controller_interface { +ControllerInterfaceBase::~ControllerInterfaceBase() +{ + // check if node is initialized and we still have a valid context + if ( + node_.get() && + get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED && + rclcpp::ok()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Calling shutdown transition of controller node '%s' due to destruction.", + get_node()->get_name()); + node_->shutdown(); + } +} + return_type ControllerInterfaceBase::init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & node_namespace, const rclcpp::NodeOptions & node_options) @@ -52,6 +68,11 @@ return_type ControllerInterfaceBase::init( break; case LifecycleNodeInterface::CallbackReturn::ERROR: case LifecycleNodeInterface::CallbackReturn::FAILURE: + RCLCPP_DEBUG( + get_node()->get_logger(), + "Calling shutdown transition of controller node '%s' due to init failure.", + get_node()->get_name()); + node_->shutdown(); return return_type::ERROR; } @@ -158,6 +179,10 @@ void ControllerInterfaceBase::release_interfaces() const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const { + if (!node_.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet!"); + } return node_->get_current_state(); } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 4204d32f45..c5197becdc 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -36,9 +36,12 @@ TEST(TestableControllerInterface, init) rclcpp::init(argc, argv); TestableControllerInterface controller; + const TestableControllerInterface & const_controller = controller; // try to get node when not initialized ASSERT_THROW(controller.get_node(), std::runtime_error); + ASSERT_THROW(const_controller.get_node(), std::runtime_error); + ASSERT_THROW(controller.get_lifecycle_state(), std::runtime_error); // initialize, create node const auto node_options = controller.define_custom_node_options(); @@ -46,6 +49,8 @@ TEST(TestableControllerInterface, init) controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); + ASSERT_NO_THROW(const_controller.get_node()); + ASSERT_NO_THROW(controller.get_lifecycle_state()); // update_rate is set to controller_manager's rate ASSERT_EQ(controller.get_update_rate(), 10u); @@ -54,6 +59,7 @@ TEST(TestableControllerInterface, init) controller.configure(); ASSERT_EQ(controller.get_update_rate(), 10u); + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -80,6 +86,8 @@ TEST(TestableControllerInterface, setting_negative_update_rate_in_configure) // The configure should fail and the update rate should stay the same ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ(controller.get_update_rate(), 1000u); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -149,6 +157,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) ASSERT_EQ(controller.get_update_rate(), 623u); executor->cancel(); + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -166,6 +175,8 @@ TEST(TestableControllerInterfaceInitError, init_with_error) controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); rclcpp::shutdown(); } @@ -183,6 +194,8 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); rclcpp::shutdown(); } diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 5829bcbcdd..eb60c19aca 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -41,7 +41,7 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), controller_interface::return_type::OK); // checks that the node options have been updated @@ -54,6 +54,8 @@ TEST(ControllerWithOption, init_with_overrides) EXPECT_EQ(controller.params["parameter1"], 1.); EXPECT_EQ(controller.params["parameter2"], 2.); EXPECT_EQ(controller.params["parameter3"], 3.); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -68,7 +70,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters) controller_node_options.arguments( {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.", "-p", "parameter_list.parameter3:=3."}); - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller_node_options), controller_interface::return_type::OK); // checks that the node options have been updated @@ -81,6 +83,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters) EXPECT_EQ(controller.params["parameter1"], 1.); EXPECT_EQ(controller.params["parameter2"], 2.); EXPECT_EQ(controller.params["parameter3"], 3.); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -96,7 +100,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file) std::cerr << params_file_path << std::endl; auto controller_node_options = controller.define_custom_node_options(); controller_node_options.arguments({"--ros-args", "--params-file", params_file_path}); - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller_node_options), controller_interface::return_type::OK); // checks that the node options have been updated @@ -112,6 +116,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file) bool use_sim_time(true); controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); ASSERT_FALSE(use_sim_time); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -130,7 +136,7 @@ TEST( controller_node_options.arguments( {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785", "-p", "use_sim_time:=true"}); - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller_node_options), controller_interface::return_type::OK); // checks that the node options have been updated @@ -146,6 +152,8 @@ TEST( bool use_sim_time(false); controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); ASSERT_TRUE(use_sim_time); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -157,7 +165,7 @@ TEST(ControllerWithOption, init_without_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), controller_interface::return_type::ERROR); // checks that the node options have been updated @@ -166,5 +174,6 @@ TEST(ControllerWithOption, init_without_overrides) EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); // checks that no parameter has been declared from overrides EXPECT_EQ(controller.params.size(), 0u); + rclcpp::shutdown(); } diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp index 7d03eb20ca..8b0b6e62d8 100644 --- a/joint_limits/test/joint_limits_rosparam_test.cpp +++ b/joint_limits/test/joint_limits_rosparam_test.cpp @@ -17,6 +17,7 @@ #include #include "joint_limits/joint_limits_rosparam.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" class JointLimitsRosParamTest : public ::testing::Test @@ -294,7 +295,17 @@ class JointLimitsLifecycleNodeUndeclaredRosParamTest : public ::testing::Test lifecycle_node_ = rclcpp_lifecycle::LifecycleNode::make_shared("JointLimitsRosparamTestNode"); } - void TearDown() { lifecycle_node_.reset(); } + void TearDown() + { + if ( + lifecycle_node_->get_current_state().id() != + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED && + rclcpp::ok()) + { + lifecycle_node_->shutdown(); + } + lifecycle_node_.reset(); + } protected: rclcpp_lifecycle::LifecycleNode::SharedPtr lifecycle_node_;