diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 44128f633e..1317420ecb 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -45,6 +45,7 @@ target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDIN add_library(joint_saturation_limiter SHARED src/joint_saturation_limiter.cpp + src/joint_range_limiter.cpp ) target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) target_include_directories(joint_saturation_limiter PUBLIC @@ -93,6 +94,15 @@ if(BUILD_TESTING) rclcpp ) + ament_add_gtest(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_saturation_limiter + pluginlib + rclcpp + ) + endif() install( diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index efaf529724..62f2a6e4d2 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -30,9 +30,8 @@ namespace joint_limits { -using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint; -template +template class JointLimiterInterface { public: @@ -70,55 +69,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) { @@ -130,6 +132,31 @@ class JointLimiterInterface return result; } + /** + * Wrapper init method that accepts the joint names and their limits directly + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, const std::vector & 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; + 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. @@ -179,19 +206,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. - */ - JOINT_LIMITS_PUBLIC virtual bool enforce(std::vector & desired_joint_states) - { - joint_limits_ = *(updated_limits_.readFromRT()); - return on_enforce(desired_joint_states); - } - protected: /** \brief Method is realized by an implementation. * @@ -222,17 +236,21 @@ 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. */ - JOINT_LIMITS_PUBLIC 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_; diff --git a/joint_limits/include/joint_limits/joint_limiter_struct.hpp b/joint_limits/include/joint_limits/joint_limiter_struct.hpp new file mode 100644 index 0000000000..c4d7440603 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limiter_struct.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__JOINT_LIMITER_STRUCT_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_STRUCT_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__JOINT_LIMITER_STRUCT_HPP_ diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 733178d695..9dbebb997b 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -36,8 +36,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 */ @@ -49,8 +49,9 @@ class JointSaturationLimiter : public JointLimiterInterface JOINT_LIMITS_PUBLIC bool on_init() override { return true; } JOINT_LIMITS_PUBLIC bool on_configure( - const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) override + const JointLimitsStateDataType & current_joint_states) override { + prev_command_ = current_joint_states; return true; } @@ -70,33 +71,23 @@ class JointSaturationLimiter : public JointLimiterInterface * \returns true if limits are enforced, otherwise false. */ JOINT_LIMITS_PUBLIC bool on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, - const rclcpp::Duration & dt) override; - - /** \brief Enforce joint limits to desired arbitrary physical quantity. - * Additional, commonly used method for enforcing limits by clamping desired input value. - * The limit is defined in effort limits of the `joint::limits/JointLimit` structure. - * - * If `has_effort_limits` is set to false, the limits will be not enforced to a joint. - * - * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the - * limits. \returns true if limits are enforced, otherwise false. - */ - JOINT_LIMITS_PUBLIC bool on_enforce(std::vector & desired_joint_states) override; + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override; private: 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/joint_limiters.xml b/joint_limits/joint_limiters.xml index a49d88fbc9..72dccfdc29 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -1,11 +1,18 @@ - + Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. + + + Simple joint range limiter using clamping approach with the parsed 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..872416562c --- /dev/null +++ b/joint_limits/src/joint_range_limiter.cpp @@ -0,0 +1,317 @@ +// 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/joint_limiter_struct.hpp" +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +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); + } + return pos_limits; +} + +std::pair compute_position_limits( + const joint_limits::JointLimits & limits, const joint_limits::SoftJointLimits & soft_limits, + double prev_command_pos, double dt) +{ + std::pair pos_limits({limits.min_position, limits.max_position}); + + // velocity bounds + double soft_min_vel = -limits.max_velocity; + double soft_max_vel = limits.max_velocity; + + if (limits.has_position_limits) + { + soft_min_vel = std::clamp( + -soft_limits.k_position * (prev_command_pos - soft_limits.min_position), -limits.max_velocity, + limits.max_velocity); + soft_max_vel = std::clamp( + -soft_limits.k_position * (prev_command_pos - soft_limits.max_position), -limits.max_velocity, + limits.max_velocity); + } + // Position bounds + pos_limits.first = prev_command_pos + soft_min_vel * dt; + pos_limits.second = prev_command_pos + soft_max_vel * dt; + return pos_limits; +} + +std::pair compute_velocity_limits( + const std::string & joint_name, const joint_limits::JointLimits & limits, + 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 max_vel_with_pos_limits = (limits.max_position - act_pos.value()) / dt; + const double min_vel_with_pos_limits = (limits.min_position - act_pos.value()) / 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 (act_pos.value() > limits.max_position || act_pos.value() < limits.min_position) + { + RCLCPP_ERROR_ONCE( + rclcpp::get_logger("joint_limiter_interface"), + "Joint position is out of bounds for the joint : '%s'. Joint velocity limits will be " + "restrictred 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); + } + 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; + } + } + return eff_limits; +} + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; + +namespace joint_limits +{ + +template <> +bool JointSaturationLimiter::on_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, 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()) + { + // limiting acc or dec function + auto apply_acc_or_dec_limit = [&](const double max_acc_or_dec, double & acc) -> bool + { + if (std::fabs(acc) > max_acc_or_dec) + { + acc = std::copysign(max_acc_or_dec, acc); + return true; + } + else + { + return false; + } + }; + + // check if decelerating - if velocity is changing toward 0 + double desired_acc = desired.acceleration.value(); + if ( + joint_limits.has_deceleration_limits && + ((desired.acceleration.value() < 0 && actual.velocity.has_value() && + actual.velocity.value() > 0) || + (desired.acceleration.value() > 0 && actual.velocity.has_value() && + actual.velocity.value() < 0))) + { + // limit deceleration + limits_enforced = + limits_enforced || apply_acc_or_dec_limit(joint_limits.max_deceleration, desired_acc); + } + else + { + // limit acceleration (fallback to acceleration if no deceleration limits) + if (joint_limits.has_acceleration_limits) + { + limits_enforced = + apply_acc_or_dec_limit(joint_limits.max_acceleration, desired_acc) || limits_enforced; + } + } + desired.acceleration = desired_acc; + } + + 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< + joint_limits::JointLimits, joint_limits::JointControlInterfacesData> + JointInterfacesSaturationLimiter; +typedef joint_limits::JointLimiterInterface< + joint_limits::JointLimits, joint_limits::JointControlInterfacesData> + 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 5020cab27b..31a0d91a54 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -27,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) { @@ -427,52 +427,18 @@ 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 +typedef std::map int_map; +typedef joint_limits::JointSaturationLimiter< + joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint> + JointTrajectoryPointSaturationLimiter; +typedef joint_limits::JointLimiterInterface< + joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint> + JointTrajectoryPointLimiterInterfaceBase; PLUGINLIB_EXPORT_CLASS( - joint_limits::JointSaturationLimiter, - joint_limits::JointLimiterInterface) + JointTrajectoryPointSaturationLimiter, JointTrajectoryPointLimiterInterfaceBase) 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..b010b4561c --- /dev/null +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -0,0 +1,653 @@ +// 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 "test_joint_range_limiter.hpp" +#include + +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); + + // 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); + 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(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_range_limiter.hpp b/joint_limits/test/test_joint_range_limiter.hpp new file mode 100644 index 0000000000..eadbf08a6b --- /dev/null +++ b/joint_limits/test/test_joint_range_limiter.hpp @@ -0,0 +1,123 @@ +// 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_RANGE_LIMITER_HPP_ +#define TEST_JOINT_RANGE_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limiter_struct.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< + joint_limits::JointLimits, joint_limits::JointControlInterfacesData>; + +class JointSaturationLimiterTest : public ::testing::Test +{ +public: + void SetUp() override + { + node_name_ = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + } + + void SetupNode(const std::string node_name = "") + { + if (!node_name.empty()) + { + node_name_ = node_name; + } + node_ = std::make_shared(node_name_); + } + + 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}, 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; + } + + JointSaturationLimiterTest() + : joint_limiter_type_("joint_limits/JointInterfacesSaturationLimiter"), + joint_limiter_loader_( + "joint_limits", + "joint_limits::JointLimiterInterface") + { + } + + void TearDown() override { node_.reset(); } + +protected: + std::string node_name_; + rclcpp::Node::SharedPtr node_; + std::vector joint_names_; + size_t num_joints_; + std::unique_ptr joint_limiter_; + std::string joint_limiter_type_; + pluginlib::ClassLoader joint_limiter_loader_; + + joint_limits::JointControlInterfacesData last_commanded_state_; + joint_limits::JointControlInterfacesData desired_state_; + joint_limits::JointControlInterfacesData actual_state_; +}; + +#endif // TEST_JOINT_RANGE_LIMITER_HPP_ diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp index e78c4b8994..72cecfcbf8 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 950b611109..ebb11ecf27 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -34,7 +34,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< + joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint>; class JointSaturationLimiterTest : public ::testing::Test { @@ -82,9 +83,11 @@ 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") { }