From d8a21c4e5effa2ac3b75a3dffbc6b48471e18a1f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 4 Jan 2025 14:59:54 +0100 Subject: [PATCH] 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; +}