From 011fabeebd66845343eca6e13f462d1d77eeaad6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 6 Apr 2024 23:53:31 +0200 Subject: [PATCH 01/71] make JointTrajectoryPoint as an input through the template argument --- .../joint_limits/joint_limiter_interface.hpp | 3 +-- .../joint_limits/joint_saturation_limiter.hpp | 13 +++++++------ joint_limits/joint_limiters.xml | 4 ++-- joint_limits/src/joint_saturation_limiter.cpp | 16 ++++++++++++---- .../test/test_joint_saturation_limiter.hpp | 7 +++++-- 5 files changed, 27 insertions(+), 16 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 17bffed021..d019a528c2 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -29,9 +29,8 @@ namespace joint_limits { -using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint; -template +template class JointLimiterInterface { public: diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index a69cc2791c..3118009d12 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 */ @@ -86,14 +86,15 @@ class JointSaturationLimiter : public JointLimiterInterface rclcpp::Clock::SharedPtr clock_; }; -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..14857c9e82 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -1,8 +1,8 @@ + type="JointSaturationLimiterTrajectoryPoint" + base_class_type="joint_limits::JointLimiterInterface<joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint>"> Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index f2a3adae1e..b3a6a940dc 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -26,7 +26,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,7 +427,8 @@ bool JointSaturationLimiter::on_enforce( } template <> -bool JointSaturationLimiter::on_enforce(std::vector & desired_joint_states) +bool JointSaturationLimiter::on_enforce( + std::vector & desired_joint_states) { std::vector limited_joints_effort; bool limits_enforced = false; @@ -472,6 +473,13 @@ bool JointSaturationLimiter::on_enforce(std::vector & desir #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, trajectory_msgs::msg::JointTrajectoryPoint> + JointSaturationLimiterTrajectoryPoint; +typedef joint_limits::JointLimiterInterface< + joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint> + JointLimiterInterfaceBaseTrajectoryPoint; PLUGINLIB_EXPORT_CLASS( - joint_limits::JointSaturationLimiter, - joint_limits::JointLimiterInterface) + JointSaturationLimiterTrajectoryPoint, JointLimiterInterfaceBaseTrajectoryPoint) diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp index 20097ae591..337ae61380 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< + joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint>; class JointSaturationLimiterTest : public ::testing::Test { @@ -83,7 +84,9 @@ class JointSaturationLimiterTest : public ::testing::Test JointSaturationLimiterTest() : joint_limiter_type_("joint_limits/JointSaturationLimiter"), joint_limiter_loader_( - "joint_limits", "joint_limits::JointLimiterInterface") + "joint_limits", + "joint_limits::JointLimiterInterface") { } From 9f34da1fa7633a791ddebd70183c15c4c1ea210a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 09:48:39 +0200 Subject: [PATCH 02/71] check for node parameter interface before declaring and getting the parameters --- .../joint_limits/joint_limiter_interface.hpp | 83 ++++++++++--------- 1 file changed, 43 insertions(+), 40 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index d019a528c2..862df276e7 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -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 (node_param_itf_) { - 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) { From cd7abd003a0d4eab5640e245d96f6f166e35028a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 09:52:12 +0200 Subject: [PATCH 03/71] Added methods to check is the parameter and logging interfaces are set or not --- .../joint_limits/joint_limiter_interface.hpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 862df276e7..8ae29384cd 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -68,7 +68,7 @@ class JointLimiterInterface // TODO(destogl): get limits from URDF // Initialize and get joint limits from parameter server - if (node_param_itf_) + if (has_parameter_interface()) { for (size_t i = 0; i < number_of_joints_; ++i) { @@ -235,6 +235,22 @@ class JointLimiterInterface */ JOINT_LIMITS_PUBLIC virtual bool on_enforce(std::vector & desired_joint_states) = 0; + /** \brief Checks if the logging interface is set. + * \returns true if the logging interface is available, otherwise false. + */ + 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. + */ + bool has_parameter_interface() const + { + return node_param_itf_ != nullptr; + } + size_t number_of_joints_; std::vector joint_names_; std::vector joint_limits_; From 797076dc06f6869d0cbd37eca4014106d98972a2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 10:29:24 +0200 Subject: [PATCH 04/71] add an init method parsing the limits directly in the init method --- .../joint_limits/joint_limiter_interface.hpp | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 8ae29384cd..122fa1502a 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -131,6 +131,24 @@ 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_); + + return on_init(); + } + /** * Wrapper init method that accepts pointer to the Node. * For details see other init method. @@ -238,18 +256,12 @@ class JointLimiterInterface /** \brief Checks if the logging interface is set. * \returns true if the logging interface is available, otherwise false. */ - bool has_logging_interface() const - { - return node_logging_itf_ != nullptr; - } + 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. */ - bool has_parameter_interface() const - { - return node_param_itf_ != nullptr; - } + bool has_parameter_interface() const { return node_param_itf_ != nullptr; } size_t number_of_joints_; std::vector joint_names_; From 8301b59dda46ebd423c48f8dec6e0573826be4b8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 10:49:01 +0200 Subject: [PATCH 05/71] update the joint state limiter to use template typename in the header file --- .../include/joint_limits/joint_saturation_limiter.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 3118009d12..bec9f5a7e2 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -46,7 +46,7 @@ class JointSaturationLimiter : public JointLimiterInterface Date: Sun, 7 Apr 2024 11:39:07 +0200 Subject: [PATCH 06/71] check the size of the joint limits and the joint names in the init method --- .../include/joint_limits/joint_limiter_interface.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 122fa1502a..d36a2b5edd 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -146,7 +146,14 @@ class JointLimiterInterface node_logging_itf_ = logging_itf; updated_limits_.writeFromNonRT(joint_limits_); - return on_init(); + 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(); } /** From ea9bda450ae42b0e08f55d7120bcac625d7970e4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 16:57:10 +0200 Subject: [PATCH 07/71] added joint range limiter library for individual joints control --- joint_limits/CMakeLists.txt | 14 ++ .../joint_limits/joint_limiter_struct.hpp | 66 +++++++ .../joint_limits/joint_range_limiter.hpp | 107 +++++++++++ joint_limits/joint_limiters.xml | 9 + joint_limits/src/joint_range_limiter.cpp | 175 ++++++++++++++++++ 5 files changed, 371 insertions(+) create mode 100644 joint_limits/include/joint_limits/joint_limiter_struct.hpp create mode 100644 joint_limits/include/joint_limits/joint_range_limiter.hpp create mode 100644 joint_limits/src/joint_range_limiter.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 8be804fa23..fff77a7ad0 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -57,6 +57,19 @@ ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_saturation_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") +add_library(joint_range_limiter SHARED + src/joint_range_limiter.cpp +) +target_compile_features(joint_range_limiter PUBLIC cxx_std_17) +target_include_directories(joint_range_limiter PUBLIC + $ + $ +) +ament_target_dependencies(joint_range_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_range_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") + pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) if(BUILD_TESTING) @@ -104,6 +117,7 @@ install(TARGETS joint_limits joint_limiter_interface joint_saturation_limiter + joint_range_limiter EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib 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_range_limiter.hpp b/joint_limits/include/joint_limits/joint_range_limiter.hpp new file mode 100644 index 0000000000..e53a13aaaf --- /dev/null +++ b/joint_limits/include/joint_limits/joint_range_limiter.hpp @@ -0,0 +1,107 @@ +// 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_RANGE_LIMITER_HPP_ +#define JOINT_LIMITS__JOINT_RANGE_LIMITER_HPP_ + +#include +#include +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" + +namespace joint_limits +{ +/** + * Joint Range Limiter limits joints' position, velocity, effort, acceleration and jerk by clamping + * values to its minimal and maximal allowed values. Since the position, velocity, effort, + * acceleration and jerk are variables in physical relation, it might be that some values are + * limited lower then specified limit. For example, if a joint is close to its position limit, + * velocity, effort, acceleration and jerk will be reduced accordingly. + */ +template +class JointRangeLimiter : public JointLimiterInterface +{ +public: + /** \brief Constructor */ + JOINT_LIMITS_PUBLIC JointRangeLimiter(); + + /** \brief Destructor */ + JOINT_LIMITS_PUBLIC ~JointRangeLimiter(); + + JOINT_LIMITS_PUBLIC bool on_init() override; + + JOINT_LIMITS_PUBLIC bool on_configure( + const JointLimitsStateDataType & current_joint_states) override + { + prev_command_ = current_joint_states; + return true; + } + + /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. + * Class implements this method accepting following data types: + * - JointInterfacesCommandLimiterData: limiting position, velocity, effort, acceleration and + * jerk; + * + * Implementation of saturation approach for joints with position, velocity or acceleration limits + * and values. First, position limits are checked to adjust desired velocity accordingly, then + * velocity and finally acceleration. + * The method support partial existence of limits, e.g., missing position limits for continuous + * joins. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC bool on_enforce( + JointLimitsStateDataType & actual, JointLimitsStateDataType & desired, + const rclcpp::Duration & dt) override; + + /** \brief Enforce joint limits to desired arbitrary physical quantity. + * Additional, commonly used method for enforcing limits by clamping desired input value. + * The limit is defined in effort limits of the `joint::limits/JointLimit` structure. + * + * If `has_effort_limits` is set to false, the limits will be not enforced to a joint. + * + * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the + * limits. \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC bool on_enforce(std::vector & /*desired_joint_states*/) override; + +private: + rclcpp::Clock::SharedPtr clock_; + JointLimitsStateDataType prev_command_; +}; + +template +JointRangeLimiter::JointRangeLimiter() +: JointLimiterInterface() +{ + clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); +} + +template +JointRangeLimiter::~JointRangeLimiter() +{ +} +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_RANGE_LIMITER_HPP_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index 14857c9e82..906e19094f 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -8,4 +8,13 @@ + + + + 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..b4a2a6e953 --- /dev/null +++ b/joint_limits/src/joint_range_limiter.cpp @@ -0,0 +1,175 @@ +// 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_range_limiter.hpp" + +#include +#include "joint_limits/joint_limiter_struct.hpp" +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +std::pair compute_position_limits( + joint_limits::JointLimits limits, double prev_command_pos, double dt) +{ + std::pair pos_limits({limits.min_position, limits.max_position}); + if (limits.has_velocity_limits) + { + const double delta_pos = limits.max_velocity * dt; + pos_limits.first = std::max(prev_command_pos - delta_pos, pos_limits.first); + pos_limits.second = std::min(prev_command_pos + delta_pos, pos_limits.second); + } + return pos_limits; +} + +std::pair compute_position_limits( + joint_limits::JointLimits limits, 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( + joint_limits::JointLimits limits, double prev_command, double dt) +{ + std::pair vel_limits({-limits.max_velocity, limits.max_velocity}); + if (limits.has_acceleration_limits) + { + const double delta_vel = limits.max_acceleration * dt; + vel_limits.first = std::max(prev_command - delta_vel, vel_limits.first); + vel_limits.second = std::min(prev_command + delta_vel, vel_limits.second); + } + return vel_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 JointRangeLimiter::on_init() +{ + const bool result = (number_of_joints_ != 1); + if (!result && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: The JointRangeLimiter expects the number of joints to be 1, but given : %zu", + number_of_joints_); + } + return result; +} + +template <> +bool JointRangeLimiter::on_enforce(std::vector &) +{ + if (has_logging_interface()) + { + RCLCPP_WARN( + node_logging_itf_->get_logger(), + "JointRangeLimiter::on_enforce" + "(std::vector & desired_joint_states) is not needed for this limiter."); + } + return false; +} + +template <> +bool JointRangeLimiter::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]; + if (!prev_command_.has_data()) + { + prev_command_ = actual; + } + + if (desired.has_position()) + { + const auto limits = + compute_position_limits(joint_limits, prev_command_.position.value(), dt_seconds); + desired.position = std::clamp(desired.position.value(), limits.first, limits.second); + } + + if (desired.has_velocity()) + { + const auto limits = + compute_velocity_limits(joint_limits, prev_command_.velocity.value(), dt_seconds); + desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); + } + + if (desired.has_effort()) + { + desired.effort = + std::clamp(desired.effort.value(), -joint_limits.max_effort, joint_limits.max_effort); + } + + if (desired.has_acceleration()) + { + desired.acceleration = std::clamp( + desired.acceleration.value(), joint_limits.max_deceleration, joint_limits.max_acceleration); + } + + if (desired.has_jerk()) + { + desired.jerk = std::clamp(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk); + } + + 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::JointRangeLimiter< + joint_limits::JointLimits, joint_limits::JointControlInterfacesData> + JointInterfacesRangeLimiter; +typedef joint_limits::JointLimiterInterface< + joint_limits::JointLimits, joint_limits::JointControlInterfacesData> + JointInterfacesRangeLimiterBase; +PLUGINLIB_EXPORT_CLASS(JointInterfacesRangeLimiter, JointInterfacesRangeLimiterBase) From 42fb8c7dc670bdd7c41bd6c22aee2fe9e4396de2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 20:14:31 +0200 Subject: [PATCH 08/71] added compute_effort_limits method --- joint_limits/src/joint_range_limiter.cpp | 34 ++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index b4a2a6e953..14bea59cac 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -72,6 +72,35 @@ std::pair compute_velocity_limits( return vel_limits; } +std::pair compute_effort_limits( + joint_limits::JointLimits limits, double act_pos, double act_vel, double /*dt*/) +{ + std::pair eff_limits({-limits.max_effort, limits.max_effort}); + if (limits.has_position_limits) + { + if ((act_pos <= limits.min_position) && (act_vel <= 0.0)) + { + eff_limits.first = 0.0; + } + else if ((act_pos >= limits.max_position) && (act_vel >= 0.0)) + { + eff_limits.second = 0.0; + } + } + if (limits.has_velocity_limits) + { + if (act_vel < -limits.max_velocity) + { + eff_limits.first = 0.0; + } + else if (act_vel > 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; @@ -142,8 +171,9 @@ bool JointRangeLimiter::on_enforce( if (desired.has_effort()) { - desired.effort = - std::clamp(desired.effort.value(), -joint_limits.max_effort, joint_limits.max_effort); + const auto limits = compute_effort_limits( + joint_limits, actual.position.value(), actual.velocity.value(), dt_seconds); + desired.effort = std::clamp(desired.effort.value(), limits.first, limits.second); } if (desired.has_acceleration()) From a78046540f62e4acf3f5655e8abdddaf23f9659f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 20:20:30 +0200 Subject: [PATCH 09/71] update the velocity limits based on the position of the joint --- joint_limits/src/joint_range_limiter.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 14bea59cac..b7b50cbe09 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -60,14 +60,21 @@ std::pair compute_position_limits( } std::pair compute_velocity_limits( - joint_limits::JointLimits limits, double prev_command, double dt) + joint_limits::JointLimits limits, double act_pos, double prev_command_vel, double dt) { std::pair vel_limits({-limits.max_velocity, limits.max_velocity}); + if (limits.has_position_limits) + { + const double max_vel_with_pos_limits = (limits.max_position - act_pos) / dt; + const double min_vel_with_pos_limits = (limits.min_position - act_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 (limits.has_acceleration_limits) { const double delta_vel = limits.max_acceleration * dt; - vel_limits.first = std::max(prev_command - delta_vel, vel_limits.first); - vel_limits.second = std::min(prev_command + delta_vel, vel_limits.second); + vel_limits.first = std::max(prev_command_vel - delta_vel, vel_limits.first); + vel_limits.second = std::min(prev_command_vel + delta_vel, vel_limits.second); } return vel_limits; } @@ -164,8 +171,8 @@ bool JointRangeLimiter::on_enforce( if (desired.has_velocity()) { - const auto limits = - compute_velocity_limits(joint_limits, prev_command_.velocity.value(), dt_seconds); + const auto limits = compute_velocity_limits( + joint_limits, actual.position.value(), prev_command_.velocity.value(), dt_seconds); desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); } From 088fa6da88eb15d01243bc1d7f1150bfa806bae5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 22:17:30 +0200 Subject: [PATCH 10/71] fix the limits clamping in the methods --- joint_limits/src/joint_range_limiter.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index b7b50cbe09..f1ed31b8d7 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -62,7 +62,9 @@ std::pair compute_position_limits( std::pair compute_velocity_limits( joint_limits::JointLimits limits, double act_pos, double prev_command_vel, double dt) { - std::pair vel_limits({-limits.max_velocity, limits.max_velocity}); + 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) { const double max_vel_with_pos_limits = (limits.max_position - act_pos) / dt; @@ -82,7 +84,9 @@ std::pair compute_velocity_limits( std::pair compute_effort_limits( joint_limits::JointLimits limits, double act_pos, double act_vel, double /*dt*/) { - std::pair eff_limits({-limits.max_effort, limits.max_effort}); + 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) { if ((act_pos <= limits.min_position) && (act_vel <= 0.0)) @@ -186,7 +190,7 @@ bool JointRangeLimiter::on_enforce( if (desired.has_acceleration()) { desired.acceleration = std::clamp( - desired.acceleration.value(), joint_limits.max_deceleration, joint_limits.max_acceleration); + desired.acceleration.value(), -joint_limits.max_deceleration, joint_limits.max_acceleration); } if (desired.has_jerk()) From 28f7e3ed47ca5e244972886a8bd21080f5f77ba1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 22:28:56 +0200 Subject: [PATCH 11/71] Add proper acceleration limits bounding --- joint_limits/src/joint_range_limiter.cpp | 35 ++++++++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index f1ed31b8d7..75f9aa2f16 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -189,8 +189,39 @@ bool JointRangeLimiter::on_enforce( if (desired.has_acceleration()) { - desired.acceleration = std::clamp( - desired.acceleration.value(), -joint_limits.max_deceleration, joint_limits.max_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.value() > 0) || + (desired.acceleration.value() > 0 && actual.velocity.value() < 0))) + { + // limit deceleration + 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) + { + apply_acc_or_dec_limit(joint_limits.max_acceleration, desired_acc); + } + } + desired.acceleration = desired_acc; } if (desired.has_jerk()) From 0944328fe10b6e3682b9f5c394546ba9f3e4278b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 23:08:03 +0200 Subject: [PATCH 12/71] remove the enforce methods of the double vector as it can be handled now by types --- .../joint_limits/joint_limiter_interface.hpp | 25 ----------- .../joint_limits/joint_range_limiter.hpp | 11 ----- .../joint_limits/joint_saturation_limiter.hpp | 11 ----- joint_limits/src/joint_range_limiter.cpp | 13 ------ joint_limits/src/joint_saturation_limiter.cpp | 44 +------------------ .../test/test_joint_saturation_limiter.cpp | 42 ------------------ 6 files changed, 1 insertion(+), 145 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index d36a2b5edd..f09b13d4be 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -205,19 +205,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. * @@ -248,18 +235,6 @@ class JointLimiterInterface JointLimitsStateDataType & current_joint_states, JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; - /** \brief Method is realized by an implementation. - * - * Filter-specific implementation of the joint limits enforce algorithm for single physical - * quantity. - * This method might use "effort" limits since they are often used as wild-card. - * Check the documentation of the exact implementation for more details. - * - * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. - * \returns true if limits are enforced, otherwise false. - */ - JOINT_LIMITS_PUBLIC virtual bool on_enforce(std::vector & desired_joint_states) = 0; - /** \brief Checks if the logging interface is set. * \returns true if the logging interface is available, otherwise false. */ diff --git a/joint_limits/include/joint_limits/joint_range_limiter.hpp b/joint_limits/include/joint_limits/joint_range_limiter.hpp index e53a13aaaf..9d75ba6ad4 100644 --- a/joint_limits/include/joint_limits/joint_range_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_range_limiter.hpp @@ -75,17 +75,6 @@ class JointRangeLimiter : public JointLimiterInterface & /*desired_joint_states*/) override; - private: rclcpp::Clock::SharedPtr clock_; JointLimitsStateDataType prev_command_; diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index bec9f5a7e2..7ed5777605 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -70,17 +70,6 @@ class JointSaturationLimiter : public JointLimiterInterface & desired_joint_states) override; - private: rclcpp::Clock::SharedPtr clock_; }; diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 75f9aa2f16..340afef3f6 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -132,19 +132,6 @@ bool JointRangeLimiter::on_init() return result; } -template <> -bool JointRangeLimiter::on_enforce(std::vector &) -{ - if (has_logging_interface()) - { - RCLCPP_WARN( - node_logging_itf_->get_logger(), - "JointRangeLimiter::on_enforce" - "(std::vector & desired_joint_states) is not needed for this limiter."); - } - return false; -} - template <> bool JointRangeLimiter::on_enforce( JointControlInterfacesData & actual, JointControlInterfacesData & desired, diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index b3a6a940dc..a3944d021c 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -426,55 +426,13 @@ bool JointSaturationLimiter -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> JointSaturationLimiterTrajectoryPoint; 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); From dfda4b2515c475812e4f5325b6d4bd8a0b4e61c4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 23:13:07 +0200 Subject: [PATCH 13/71] Added comments to the has_logging_interface and has_parameter_interface methods --- .../include/joint_limits/joint_limiter_interface.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index f09b13d4be..601013e611 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -237,11 +237,17 @@ class JointLimiterInterface /** \brief Checks if the logging interface is set. * \returns true if the logging interface is available, 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. */ 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. + * + * * \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_parameter_interface() const { return node_param_itf_ != nullptr; } From 87714fbffe30247f13677b67851ebc63a7c0641a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 23:41:54 +0200 Subject: [PATCH 14/71] remove the joint range limiter header and reuse it from the joint saturation limiter + merge them into a single library --- joint_limits/CMakeLists.txt | 15 +-- .../joint_limits/joint_range_limiter.hpp | 96 ------------------- .../joint_limits/joint_saturation_limiter.hpp | 4 +- joint_limits/joint_limiters.xml | 6 +- joint_limits/src/joint_range_limiter.cpp | 17 ++-- 5 files changed, 15 insertions(+), 123 deletions(-) delete mode 100644 joint_limits/include/joint_limits/joint_range_limiter.hpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index fff77a7ad0..fccf4395d0 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -46,6 +46,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 @@ -57,19 +58,6 @@ ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_saturation_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") -add_library(joint_range_limiter SHARED - src/joint_range_limiter.cpp -) -target_compile_features(joint_range_limiter PUBLIC cxx_std_17) -target_include_directories(joint_range_limiter PUBLIC - $ - $ -) -ament_target_dependencies(joint_range_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(joint_range_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") - pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) if(BUILD_TESTING) @@ -117,7 +105,6 @@ install(TARGETS joint_limits joint_limiter_interface joint_saturation_limiter - joint_range_limiter EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/joint_limits/include/joint_limits/joint_range_limiter.hpp b/joint_limits/include/joint_limits/joint_range_limiter.hpp deleted file mode 100644 index 9d75ba6ad4..0000000000 --- a/joint_limits/include/joint_limits/joint_range_limiter.hpp +++ /dev/null @@ -1,96 +0,0 @@ -// 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_RANGE_LIMITER_HPP_ -#define JOINT_LIMITS__JOINT_RANGE_LIMITER_HPP_ - -#include -#include -#include -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" - -namespace joint_limits -{ -/** - * Joint Range Limiter limits joints' position, velocity, effort, acceleration and jerk by clamping - * values to its minimal and maximal allowed values. Since the position, velocity, effort, - * acceleration and jerk are variables in physical relation, it might be that some values are - * limited lower then specified limit. For example, if a joint is close to its position limit, - * velocity, effort, acceleration and jerk will be reduced accordingly. - */ -template -class JointRangeLimiter : public JointLimiterInterface -{ -public: - /** \brief Constructor */ - JOINT_LIMITS_PUBLIC JointRangeLimiter(); - - /** \brief Destructor */ - JOINT_LIMITS_PUBLIC ~JointRangeLimiter(); - - JOINT_LIMITS_PUBLIC bool on_init() override; - - JOINT_LIMITS_PUBLIC bool on_configure( - const JointLimitsStateDataType & current_joint_states) override - { - prev_command_ = current_joint_states; - return true; - } - - /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. - * Class implements this method accepting following data types: - * - JointInterfacesCommandLimiterData: limiting position, velocity, effort, acceleration and - * jerk; - * - * Implementation of saturation approach for joints with position, velocity or acceleration limits - * and values. First, position limits are checked to adjust desired velocity accordingly, then - * velocity and finally acceleration. - * The method support partial existence of limits, e.g., missing position limits for continuous - * joins. - * - * \param[in] current_joint_states current joint states a robot is in. - * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. - * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. - * \returns true if limits are enforced, otherwise false. - */ - JOINT_LIMITS_PUBLIC bool on_enforce( - JointLimitsStateDataType & actual, JointLimitsStateDataType & desired, - const rclcpp::Duration & dt) override; - -private: - rclcpp::Clock::SharedPtr clock_; - JointLimitsStateDataType prev_command_; -}; - -template -JointRangeLimiter::JointRangeLimiter() -: JointLimiterInterface() -{ - clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); -} - -template -JointRangeLimiter::~JointRangeLimiter() -{ -} -} // namespace joint_limits - -#endif // JOINT_LIMITS__JOINT_RANGE_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 7ed5777605..46dab960b0 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -46,8 +46,9 @@ class JointSaturationLimiter : public JointLimiterInterface diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index 906e19094f..5bfab98344 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -7,10 +7,8 @@ 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 index 340afef3f6..bb489f5bff 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -14,7 +14,7 @@ /// \author Sai Kishor Kothakota -#include "joint_limits/joint_range_limiter.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" #include #include "joint_limits/joint_limiter_struct.hpp" @@ -119,21 +119,22 @@ namespace joint_limits { template <> -bool JointRangeLimiter::on_init() +bool JointSaturationLimiter::on_init() { const bool result = (number_of_joints_ != 1); if (!result && has_logging_interface()) { RCLCPP_ERROR( node_logging_itf_->get_logger(), - "JointLimiter: The JointRangeLimiter expects the number of joints to be 1, but given : %zu", + "JointLimiter: The JointSaturationLimiter expects the number of joints to be 1, but given : " + "%zu", number_of_joints_); } return result; } template <> -bool JointRangeLimiter::on_enforce( +bool JointSaturationLimiter::on_enforce( JointControlInterfacesData & actual, JointControlInterfacesData & desired, const rclcpp::Duration & dt) { @@ -225,10 +226,10 @@ bool JointRangeLimiter::on_enforce( // typedefs are needed here to avoid issues with macro expansion. ref: // https://stackoverflow.com/a/8942986 -typedef joint_limits::JointRangeLimiter< +typedef joint_limits::JointSaturationLimiter< joint_limits::JointLimits, joint_limits::JointControlInterfacesData> - JointInterfacesRangeLimiter; + JointInterfacesSaturationLimiter; typedef joint_limits::JointLimiterInterface< joint_limits::JointLimits, joint_limits::JointControlInterfacesData> - JointInterfacesRangeLimiterBase; -PLUGINLIB_EXPORT_CLASS(JointInterfacesRangeLimiter, JointInterfacesRangeLimiterBase) + JointInterfacesSaturationLLimiterBase; +PLUGINLIB_EXPORT_CLASS(JointInterfacesSaturationLimiter, JointInterfacesSaturationLLimiterBase) From 7a4ce74cec2ec6b15afb72ce1910396500942699 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 23:52:56 +0200 Subject: [PATCH 15/71] rename the plugin names to be more consistent with the types the use internally --- joint_limits/joint_limiters.xml | 4 ++-- joint_limits/src/joint_range_limiter.cpp | 4 ++-- joint_limits/src/joint_saturation_limiter.cpp | 6 +++--- joint_limits/test/test_joint_saturation_limiter.hpp | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index 5bfab98344..72dccfdc29 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -1,7 +1,7 @@ - Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index bb489f5bff..af12b36abd 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -231,5 +231,5 @@ typedef joint_limits::JointSaturationLimiter< JointInterfacesSaturationLimiter; typedef joint_limits::JointLimiterInterface< joint_limits::JointLimits, joint_limits::JointControlInterfacesData> - JointInterfacesSaturationLLimiterBase; -PLUGINLIB_EXPORT_CLASS(JointInterfacesSaturationLimiter, JointInterfacesSaturationLLimiterBase) + 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 a3944d021c..317a6b7515 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -435,9 +435,9 @@ bool JointSaturationLimiter int_map; typedef joint_limits::JointSaturationLimiter< joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint> - JointSaturationLimiterTrajectoryPoint; + JointTrajectoryPointSaturationLimiter; typedef joint_limits::JointLimiterInterface< joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint> - JointLimiterInterfaceBaseTrajectoryPoint; + JointTrajectoryPointLimiterInterfaceBase; PLUGINLIB_EXPORT_CLASS( - JointSaturationLimiterTrajectoryPoint, JointLimiterInterfaceBaseTrajectoryPoint) + JointTrajectoryPointSaturationLimiter, JointTrajectoryPointLimiterInterfaceBase) diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp index 337ae61380..7ab3474ec6 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -82,7 +82,7 @@ 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 Date: Wed, 17 Apr 2024 18:59:19 +0200 Subject: [PATCH 16/71] Add first version of tests into the joint range limiter --- joint_limits/CMakeLists.txt | 9 + .../test/test_joint_range_limiter.cpp | 527 ++++++++++++++++++ .../test/test_joint_range_limiter.hpp | 116 ++++ 3 files changed, 652 insertions(+) create mode 100644 joint_limits/test/test_joint_range_limiter.cpp create mode 100644 joint_limits/test/test_joint_range_limiter.hpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index fccf4395d0..2a450328cc 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -95,6 +95,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/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp new file mode 100644 index 0000000000..dccfbd4254 --- /dev/null +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -0,0 +1,527 @@ +// 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" + +TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test JointSaturationLimiter loading + ASSERT_NO_THROW( + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); + ASSERT_NE(joint_limiter_, nullptr); +} + +// NOTE: We accept also if there is no limit defined for a joint. +TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + // initialize the limiter + std::vector joint_names = {"bar_joint"}; + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); + } +} + +TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + } +} + +// TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// // no size check occurs (yet) so expect true +// ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + +// rclcpp::Duration period(1, 0); // 1 second +// // test no desired interface +// desired_joint_states_.positions.clear(); +// desired_joint_states_.velocities.clear(); +// // currently not handled desired_joint_states_.accelerations.clear(); +// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// joint_limiter_->configure(last_commanded_state_); + +// rclcpp::Duration period(1, 0); // 1 second +// // test no position interface +// current_joint_states_.positions.clear(); +// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // also fail if out of limits +// desired_joint_states_.positions[0] = 20.0; +// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// joint_limiter_->configure(last_commanded_state_); + +// rclcpp::Duration period(1, 0); // 1 second +// // test no vel interface +// current_joint_states_.velocities.clear(); +// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); +// // also fail if out of limits +// desired_joint_states_.positions[0] = 20.0; +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(1.0, 0.0); // 1 second +// // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + +// // within limits +// desired_joint_states_.positions[0] = 1.0; +// desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well +// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if no limits applied +// CHECK_STATE_SINGLE_JOINT( +// desired_joint_states_, 0, +// 1.0, // pos unchanged +// 1.0, // vel unchanged +// 1.0 // acc = vel / 1.0 +// ); +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_with_acc) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(1.0, 0.0); // 1 second +// // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + +// // within limits +// desired_joint_states_.positions[0] = 1.0; +// desired_joint_states_.velocities[0] = 1.5; // valid pos derivative as well +// desired_joint_states_.accelerations[0] = 2.9; // valid pos derivative as well +// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if no limits applied +// CHECK_STATE_SINGLE_JOINT( +// desired_joint_states_, 0, +// 1.0, // pos unchanged +// 1.5, // vel unchanged +// 2.9 // acc = vel / 1.0 +// ); +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(1.0, 0.0); // 1 second + +// // pos/vel cmd ifs +// current_joint_states_.positions[0] = -2.0; +// desired_joint_states_.positions[0] = 1.0; +// // desired velocity exceeds although correctly computed from pos derivative +// desired_joint_states_.velocities[0] = 3.0; +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if limits applied +// CHECK_STATE_SINGLE_JOINT( +// desired_joint_states_, 0, +// 0.0, // pos = pos + max_vel * dt +// 2.0, // vel limited to max_vel +// 2.0 / 1.0 // acc set to vel change/DT +// ); + +// // check opposite velocity direction (sign copy) +// current_joint_states_.positions[0] = 2.0; +// desired_joint_states_.positions[0] = -1.0; +// // desired velocity exceeds although correctly computed from pos derivative +// desired_joint_states_.velocities[0] = -3.0; +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if vel and acc limits applied +// CHECK_STATE_SINGLE_JOINT( +// desired_joint_states_, 0, +// 0.0, // pos = pos - max_vel * dt +// -2.0, // vel limited to -max_vel +// -2.0 / 1.0 // acc set to vel change/DT +// ); +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(1.0, 0.0); // 1 second + +// // set current velocity close to limits to not be blocked by max acc to reach max +// current_joint_states_.velocities[0] = 1.9; +// // desired pos leads to vel exceeded (4.0 / sec instead of 2.0) +// desired_joint_states_.positions[0] = 4.0; +// // no vel cmd (will lead to internal computation of velocity) +// desired_joint_states_.velocities.clear(); +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if pos and acc limits applied +// ASSERT_EQ(desired_joint_states_.positions[0], 2.0); // pos limited to max_vel * DT +// // no vel cmd ifs +// ASSERT_EQ( +// desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT + +// // check opposite position and direction +// current_joint_states_.positions[0] = 0.0; +// current_joint_states_.velocities[0] = -1.9; +// desired_joint_states_.positions[0] = -4.0; +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if pos and acc limits applied +// ASSERT_EQ(desired_joint_states_.positions[0], -2.0); // pos limited to -max_vel * DT +// // no vel cmd ifs +// ASSERT_EQ( +// desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(1.0, 0.0); // 1 second + +// // vel cmd ifs +// current_joint_states_.positions[0] = -2.0; +// // set current velocity close to limits to not be blocked by max acc to reach max +// current_joint_states_.velocities[0] = 1.9; +// // no pos cmd +// desired_joint_states_.positions.clear(); +// // desired velocity exceeds +// desired_joint_states_.velocities[0] = 3.0; + +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if vel and acc limits applied +// ASSERT_EQ(desired_joint_states_.velocities[0], 2.0); // vel limited to max_vel +// // no vel cmd ifs +// ASSERT_EQ( +// desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT + +// // check opposite velocity direction +// current_joint_states_.positions[0] = 2.0; +// // set current velocity close to limits to not be blocked by max acc to reach max +// current_joint_states_.velocities[0] = -1.9; +// // desired velocity exceeds +// desired_joint_states_.velocities[0] = -3.0; + +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if vel and acc limits applied +// ASSERT_EQ(desired_joint_states_.velocities[0], -2.0); // vel limited to -max_vel +// // no vel cmd ifs +// ASSERT_EQ( +// desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(1.0, 0.0); // 1 second + +// // desired pos exceeds +// current_joint_states_.positions[0] = 5.0; +// desired_joint_states_.positions[0] = 20.0; +// // no velocity interface +// desired_joint_states_.velocities.clear(); +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if pos limits applied +// ASSERT_EQ( +// desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit) +// // no vel cmd ifs +// ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// // using 0.05 because 1.0 sec invalidates the "small dt integration" +// rclcpp::Duration period(0, 50000000); // 0.05 second + +// // close to pos limit should reduce velocity and stop +// current_joint_states_.positions[0] = 4.851; +// current_joint_states_.velocities[0] = 1.5; +// desired_joint_states_.positions[0] = 4.926; +// desired_joint_states_.velocities[0] = 1.5; + +// // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) +// std::vector expected_ret = {true, true, true, false}; +// for (auto i = 0u; i < 4; ++i) +// { +// auto previous_vel_request = desired_joint_states_.velocities[0]; +// // expect limits applied until the end stop +// ASSERT_EQ( +// joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period), +// expected_ret[i]); + +// ASSERT_LE( +// desired_joint_states_.velocities[0], +// previous_vel_request); // vel adapted to reach end-stop should be decreasing +// // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit +// // hence no max deceleration anymore... +// ASSERT_LT( +// desired_joint_states_.positions[0], +// 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits +// ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate + +// Integrate(period.seconds()); + +// ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit +// } +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(0, 50000000); + +// // desired acceleration exceeds +// current_joint_states_.positions[0] = 0.1; +// current_joint_states_.velocities[0] = 0.1; +// desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity +// desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if limits applied +// CHECK_STATE_SINGLE_JOINT( +// desired_joint_states_, 0, +// 0.11125, // pos = double integration from max acc with current state +// 0.35, // vel limited to vel + max acc * dt +// 5.0 // acc max acc +// ); +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(0, 50000000); + +// // desired acceleration exceeds +// current_joint_states_.positions[0] = 0.1; +// current_joint_states_.velocities[0] = 0.1; +// desired_joint_states_.positions[0] = +// 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc +// desired_joint_states_.velocities.clear(); +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if pos and acc limits applied +// ASSERT_NEAR( +// desired_joint_states_.positions[0], 0.111250, +// COMMON_THRESHOLD); // pos = double integration from max acc with current state +// // no vel cmd ifs +// ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(0, 50000000); + +// // desired acceleration exceeds +// current_joint_states_.positions[0] = 0.1; +// current_joint_states_.velocities[0] = 0.1; +// desired_joint_states_.positions.clear(); // = 0.125; +// desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if pos and acc limits applied +// // no pos cmd ifs +// ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt +// ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforced) +// { +// SetupNode("joint_saturation_limiter"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(0, 50000000); + +// // desired deceleration exceeds +// current_joint_states_.positions[0] = 0.3; +// current_joint_states_.velocities[0] = 0.5; +// desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity +// desired_joint_states_.velocities[0] = 0.1; // leads to acc < -max dec + +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if vel and acc limits applied +// CHECK_STATE_SINGLE_JOINT( +// desired_joint_states_, 0, +// 0.315625, // pos = double integration from max dec with current state +// 0.125, // vel limited by vel - max dec * dt +// -7.5 // acc limited by -max dec +// ); +// } +// } + +// TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) +// { +// SetupNode("joint_saturation_limiter_nodeclimit"); +// Load(); + +// if (joint_limiter_) +// { +// Init(); +// Configure(); + +// rclcpp::Duration period(0, 50000000); + +// // desired deceleration exceeds +// current_joint_states_.positions[0] = 1.0; +// current_joint_states_.velocities[0] = 1.0; +// desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity +// desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc +// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + +// // check if vel and acc limits applied +// CHECK_STATE_SINGLE_JOINT( +// desired_joint_states_, 0, +// 1.04375, // pos = double integration from max acc with current state +// 0.75, // vel limited by vel-max acc * dt +// -5.0 // acc limited to -max acc +// ); +// } +// } + +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..b847714169 --- /dev/null +++ b/joint_limits/test/test_joint_range_limiter.hpp @@ -0,0 +1,116 @@ +// 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_SATURATION_LIMITER_HPP_ +#define TEST_JOINT_SATURATION_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "joint_limits/joint_limiter_struct.hpp" + +const double COMMON_THRESHOLD = 0.0011; + +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_); + } + + void Load() + { + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + } + + void Init(const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + joint_limiter_->init(joint_names_, node_); + num_joints_ = joint_names_.size(); + last_commanded_state_.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_; + } + + void Init(const joint_limits::JointControlInterfacesData & init_state) + { + last_commanded_state_ = init_state; + joint_names_ = {last_commanded_state_.joint_name}; + joint_limiter_->init(joint_names_, node_); + num_joints_ = joint_names_.size(); + desired_state_ = last_commanded_state_; + actual_state_ = last_commanded_state_; + } + + void Configure() { 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_SATURATION_LIMITER_HPP_ From 02796bf105556b4757dcc0e12da52e435e12a8f0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 21:44:30 +0200 Subject: [PATCH 17/71] fix the init check bug --- joint_limits/src/joint_range_limiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index af12b36abd..0ceda264fc 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -121,7 +121,7 @@ namespace joint_limits template <> bool JointSaturationLimiter::on_init() { - const bool result = (number_of_joints_ != 1); + const bool result = (number_of_joints_ == 1); if (!result && has_logging_interface()) { RCLCPP_ERROR( From 55bb040fd3972aa74a4f96c8c780813085b87d58 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 22:12:54 +0200 Subject: [PATCH 18/71] Apply formatting changes --- joint_limits/src/joint_range_limiter.cpp | 2 +- .../test/test_joint_range_limiter.cpp | 4 ++-- .../test/test_joint_range_limiter.hpp | 21 ++++++++++--------- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 0ceda264fc..ea298a9fa1 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -126,7 +126,7 @@ bool JointSaturationLimiter::on_init() { RCLCPP_ERROR( node_logging_itf_->get_logger(), - "JointLimiter: The JointSaturationLimiter expects the number of joints to be 1, but given : " + "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : " "%zu", number_of_joints_); } diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index dccfbd4254..d65b823b33 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -503,8 +503,8 @@ TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) // // desired deceleration exceeds // current_joint_states_.positions[0] = 1.0; // current_joint_states_.velocities[0] = 1.0; -// desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity -// desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc +// desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased +// velocity desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc // ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); // // check if vel and acc limits applied diff --git a/joint_limits/test/test_joint_range_limiter.hpp b/joint_limits/test/test_joint_range_limiter.hpp index b847714169..2cf96d6974 100644 --- a/joint_limits/test/test_joint_range_limiter.hpp +++ b/joint_limits/test/test_joint_range_limiter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEST_JOINT_SATURATION_LIMITER_HPP_ -#define TEST_JOINT_SATURATION_LIMITER_HPP_ +#ifndef TEST_JOINT_RANGE_LIMITER_HPP_ +#define TEST_JOINT_RANGE_LIMITER_HPP_ #include @@ -21,11 +21,11 @@ #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" -#include "joint_limits/joint_limiter_struct.hpp" const double COMMON_THRESHOLD = 0.0011; @@ -62,13 +62,13 @@ class JointSaturationLimiterTest : public ::testing::Test 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; + 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_; } - + void Init(const joint_limits::JointControlInterfacesData & init_state) { last_commanded_state_ = init_state; @@ -84,8 +84,9 @@ class JointSaturationLimiterTest : public ::testing::Test 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; + 0.5 * desired_state_.acceleration.value() * dt * dt; + actual_state_.velocity = + actual_state_.velocity.value() + desired_state_.acceleration.value() * dt; } JointSaturationLimiterTest() @@ -113,4 +114,4 @@ class JointSaturationLimiterTest : public ::testing::Test joint_limits::JointControlInterfacesData actual_state_; }; -#endif // TEST_JOINT_SATURATION_LIMITER_HPP_ +#endif // TEST_JOINT_RANGE_LIMITER_HPP_ From eab6689741df3ee07d12a0136dc760c763f9903a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 22:55:41 +0200 Subject: [PATCH 19/71] update the joint range limiter test with case of only desired position availability case --- .../test/test_joint_range_limiter.cpp | 98 ++++++++++++------- .../test/test_joint_range_limiter.hpp | 24 +++-- 2 files changed, 77 insertions(+), 45 deletions(-) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index d65b823b33..4165d342e8 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -29,49 +29,75 @@ TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) { SetupNode("joint_saturation_limiter"); - Load(); - - if (joint_limiter_) - { - // initialize the limiter - std::vector joint_names = {"bar_joint"}; - ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); - } + 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"); - Load(); - - if (joint_limiter_) - { - Init(); - Configure(); - rclcpp::Duration period(0, 0); // 0 second - ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); - } -} - -// TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); + ASSERT_TRUE(Load()); -// if (joint_limiter_) -// { -// Init(); -// // no size check occurs (yet) so expect true -// ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + ASSERT_TRUE(Init()); + ASSERT_TRUE(Configure()); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); +} -// rclcpp::Duration period(1, 0); // 1 second -// // test no desired interface -// desired_joint_states_.positions.clear(); -// desired_joint_states_.velocities.clear(); -// // currently not handled desired_joint_states_.accelerations.clear(); -// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); -// } -// } +TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) +{ + 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_ = {}; + last_commanded_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.position = 4.0; + ASSERT_TRUE(desired_state_.has_position()); + ASSERT_FALSE(desired_state_.has_velocity()); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_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); + ASSERT_FALSE(desired_state_.has_velocity()); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_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); + ASSERT_FALSE(desired_state_.has_velocity()); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_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); + ASSERT_FALSE(desired_state_.has_velocity()); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_FALSE(desired_state_.has_jerk()); +} // TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) // { diff --git a/joint_limits/test/test_joint_range_limiter.hpp b/joint_limits/test/test_joint_range_limiter.hpp index 2cf96d6974..eadbf08a6b 100644 --- a/joint_limits/test/test_joint_range_limiter.hpp +++ b/joint_limits/test/test_joint_range_limiter.hpp @@ -27,7 +27,7 @@ #include "rclcpp/duration.hpp" #include "rclcpp/node.hpp" -const double COMMON_THRESHOLD = 0.0011; +const double COMMON_THRESHOLD = 1.0e-6; using JointLimiter = joint_limits::JointLimiterInterface< joint_limits::JointLimits, joint_limits::JointControlInterfacesData>; @@ -49,16 +49,16 @@ class JointSaturationLimiterTest : public ::testing::Test node_ = std::make_shared(node_name_); } - void Load() + bool Load() { joint_limiter_ = std::unique_ptr( joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + return joint_limiter_ != nullptr; } - void Init(const std::string & joint_name = "foo_joint") + bool Init(const std::string & joint_name = "foo_joint") { joint_names_ = {joint_name}; - joint_limiter_->init(joint_names_, node_); num_joints_ = joint_names_.size(); last_commanded_state_.joint_name = joint_name; last_commanded_state_.position = 0.0; @@ -67,19 +67,25 @@ class JointSaturationLimiterTest : public ::testing::Test last_commanded_state_.effort = 0.0; desired_state_ = last_commanded_state_; actual_state_ = last_commanded_state_; + return joint_limiter_->init(joint_names_, node_); } - void Init(const joint_limits::JointControlInterfacesData & init_state) + bool Init(const joint_limits::JointLimits & limits, const std::string & joint_name = "foo_joint") { - last_commanded_state_ = init_state; - joint_names_ = {last_commanded_state_.joint_name}; - joint_limiter_->init(joint_names_, node_); + 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()); } - void Configure() { joint_limiter_->configure(last_commanded_state_); } + bool Configure() { return joint_limiter_->configure(last_commanded_state_); } void Integrate(double dt) { From 7fe23369aff5a905526c2e5c55081fb580e70c82 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 22:56:10 +0200 Subject: [PATCH 20/71] Added some minor fixes in the joint range limiter --- joint_limits/src/joint_range_limiter.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index ea298a9fa1..e60ad2fcc3 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -21,6 +21,8 @@ #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( joint_limits::JointLimits limits, double prev_command_pos, double dt) { @@ -158,6 +160,7 @@ bool JointSaturationLimiter::on_enforce { const auto limits = compute_position_limits(joint_limits, prev_command_.position.value(), dt_seconds); + limits_enforced = is_limited(desired.position.value(), limits.first, limits.second); desired.position = std::clamp(desired.position.value(), limits.first, limits.second); } @@ -165,6 +168,8 @@ bool JointSaturationLimiter::on_enforce { const auto limits = compute_velocity_limits( joint_limits, actual.position.value(), prev_command_.velocity.value(), dt_seconds); + limits_enforced = + limits_enforced || is_limited(desired.velocity.value(), limits.first, limits.second); desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); } @@ -172,6 +177,8 @@ bool JointSaturationLimiter::on_enforce { const auto limits = compute_effort_limits( joint_limits, actual.position.value(), actual.velocity.value(), dt_seconds); + limits_enforced = + limits_enforced || is_limited(desired.effort.value(), limits.first, limits.second); desired.effort = std::clamp(desired.effort.value(), limits.first, limits.second); } @@ -199,14 +206,16 @@ bool JointSaturationLimiter::on_enforce (desired.acceleration.value() > 0 && actual.velocity.value() < 0))) { // limit deceleration - apply_acc_or_dec_limit(joint_limits.max_deceleration, desired_acc); + 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) { - apply_acc_or_dec_limit(joint_limits.max_acceleration, desired_acc); + limits_enforced = + limits_enforced || apply_acc_or_dec_limit(joint_limits.max_acceleration, desired_acc); } } desired.acceleration = desired_acc; @@ -214,9 +223,14 @@ bool JointSaturationLimiter::on_enforce if (desired.has_jerk()) { + limits_enforced = + limits_enforced || + is_limited(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk); desired.jerk = std::clamp(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk); } + prev_command_ = desired; + return limits_enforced; } From 926984212317c62fdd6ce913d36fa22a8213914c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 23:10:49 +0200 Subject: [PATCH 21/71] Fill the data of actual or desired fields into the previous commands --- joint_limits/src/joint_range_limiter.cpp | 50 ++++++++++++++++++- .../test/test_joint_range_limiter.cpp | 1 + 2 files changed, 50 insertions(+), 1 deletion(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index e60ad2fcc3..05834fc769 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -151,9 +151,57 @@ bool JointSaturationLimiter::on_enforce 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()) { - prev_command_ = actual; + 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()) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 4165d342e8..12e3cf2897 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -72,6 +72,7 @@ TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce ASSERT_FALSE(desired_state_.has_effort()); ASSERT_FALSE(desired_state_.has_jerk()); + // The previous command does nothing in this case as there is no velocity limits // 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)); From f94efc2b4fa63a38cdbda0f6175790d330bbabcf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 23:48:13 +0200 Subject: [PATCH 22/71] reset the prev_data_ on initialization --- joint_limits/src/joint_range_limiter.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 05834fc769..c92a4ae211 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -132,6 +132,7 @@ bool JointSaturationLimiter::on_init() "%zu", number_of_joints_); } + prev_command_ = JointControlInterfacesData(); return result; } From fa90bfae8344d266c9c605faf51c0f213d4e7c22 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 23:48:42 +0200 Subject: [PATCH 23/71] add test cases of the actual and desired position cases --- .../test/test_joint_range_limiter.cpp | 46 ++++++++++++++++++- 1 file changed, 44 insertions(+), 2 deletions(-) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 12e3cf2897..f246e814f1 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -46,7 +46,7 @@ TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); } -TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) +TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) { SetupNode("joint_saturation_limiter"); ASSERT_TRUE(Load()); @@ -62,7 +62,6 @@ TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce // Reset the desired and actual states desired_state_ = {}; actual_state_ = {}; - last_commanded_state_ = {}; rclcpp::Duration period(1, 0); // 1 second desired_state_.position = 4.0; @@ -98,6 +97,49 @@ TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce ASSERT_FALSE(desired_state_.has_acceleration()); ASSERT_FALSE(desired_state_.has_effort()); ASSERT_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); + ASSERT_TRUE(desired_state_.has_position()); + ASSERT_FALSE(desired_state_.has_velocity()); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_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); + ASSERT_TRUE(desired_state_.has_position()); + ASSERT_FALSE(desired_state_.has_velocity()); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_FALSE(desired_state_.has_jerk()); } // TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) From b9918e5187a6c4a0f4970d9e5babfa07379db2a4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 09:22:21 +0200 Subject: [PATCH 24/71] Add tests for the case of no position limits --- joint_limits/test/test_joint_range_limiter.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index f246e814f1..54442a7364 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -140,6 +140,22 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) ASSERT_FALSE(desired_state_.has_acceleration()); ASSERT_FALSE(desired_state_.has_effort()); ASSERT_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); + ASSERT_TRUE(desired_state_.has_position()); + ASSERT_FALSE(desired_state_.has_velocity()); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_FALSE(desired_state_.has_jerk()); } // TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) From 59b697d2d3418acff7f6735ad5ad06b5e82c88ae Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 10:12:17 +0200 Subject: [PATCH 25/71] add initial test cases for desired velocity only cases --- .../test/test_joint_range_limiter.cpp | 90 ++++++++++++++++++- 1 file changed, 89 insertions(+), 1 deletion(-) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 54442a7364..b5c3bf1f01 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -71,7 +71,6 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) ASSERT_FALSE(desired_state_.has_effort()); ASSERT_FALSE(desired_state_.has_jerk()); - // The previous command does nothing in this case as there is no velocity limits // 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)); @@ -158,6 +157,95 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) ASSERT_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; + ASSERT_FALSE(desired_state_.has_position()); + ASSERT_TRUE(desired_state_.has_velocity()); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_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)); + ASSERT_FALSE(desired_state_.has_position()); + ASSERT_TRUE(desired_state_.has_velocity()); + EXPECT_NEAR(desired_state_.velocity.value(), limits.max_velocity, COMMON_THRESHOLD); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_FALSE(desired_state_.has_jerk()); + + desired_state_.velocity = -5.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + ASSERT_FALSE(desired_state_.has_position()); + ASSERT_TRUE(desired_state_.has_velocity()); + EXPECT_NEAR(desired_state_.velocity.value(), -limits.max_velocity, COMMON_THRESHOLD); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_FALSE(desired_state_.has_jerk()); + + // If the desired state is within the limits, then no saturation is needed + desired_state_.velocity = 1.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + ASSERT_FALSE(desired_state_.has_position()); + ASSERT_TRUE(desired_state_.has_velocity()); + EXPECT_NEAR(desired_state_.velocity.value(), 1.0, COMMON_THRESHOLD); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_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 = + [&](double actual_position, double desired_velocity, double expected_velocity, bool is_clamped) + { + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(actual_position) + + ", desired velocity: " + std::to_string(desired_velocity) + ", expected velocity: " + + std::to_string(expected_velocity) + ", is clamped: " + std::to_string(is_clamped)); + actual_state_.position = actual_position; + desired_state_.velocity = desired_velocity; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + ASSERT_FALSE(desired_state_.has_position()); + ASSERT_TRUE(desired_state_.has_velocity()); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + ASSERT_FALSE(desired_state_.has_acceleration()); + ASSERT_FALSE(desired_state_.has_effort()); + ASSERT_FALSE(desired_state_.has_jerk()); + }; + 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(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_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) // { // SetupNode("joint_saturation_limiter"); From 372b0d4cf329031912ad2fcf69591b25f9f2bb01 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 10:12:52 +0200 Subject: [PATCH 26/71] Enforce wrt to the position limits only when the actual position value is available --- joint_limits/src/joint_range_limiter.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index c92a4ae211..204d0fdc58 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -62,23 +62,24 @@ std::pair compute_position_limits( } std::pair compute_velocity_limits( - joint_limits::JointLimits limits, double act_pos, double prev_command_vel, double dt) + 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) + if (limits.has_position_limits && act_pos.has_value()) { - const double max_vel_with_pos_limits = (limits.max_position - act_pos) / dt; - const double min_vel_with_pos_limits = (limits.min_position - act_pos) / dt; + 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 (limits.has_acceleration_limits) + 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 - delta_vel, vel_limits.first); - vel_limits.second = std::min(prev_command_vel + delta_vel, vel_limits.second); + 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; } @@ -215,8 +216,8 @@ bool JointSaturationLimiter::on_enforce if (desired.has_velocity()) { - const auto limits = compute_velocity_limits( - joint_limits, actual.position.value(), prev_command_.velocity.value(), dt_seconds); + const auto limits = + compute_velocity_limits(joint_limits, actual.position, prev_command_.velocity, dt_seconds); limits_enforced = limits_enforced || is_limited(desired.velocity.value(), limits.first, limits.second); desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); From 560d1bc29b15c8fa23bc91b1d252880d7f6ea55d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 10:17:27 +0200 Subject: [PATCH 27/71] change some asserts to expects --- .../test/test_joint_range_limiter.cpp | 114 +++++++++--------- 1 file changed, 57 insertions(+), 57 deletions(-) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index b5c3bf1f01..2f0a7e6f09 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -65,37 +65,37 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) rclcpp::Duration period(1, 0); // 1 second desired_state_.position = 4.0; - ASSERT_TRUE(desired_state_.has_position()); - ASSERT_FALSE(desired_state_.has_velocity()); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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); - ASSERT_FALSE(desired_state_.has_velocity()); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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); - ASSERT_FALSE(desired_state_.has_velocity()); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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); - ASSERT_FALSE(desired_state_.has_velocity()); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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; @@ -107,11 +107,11 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) 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); - ASSERT_TRUE(desired_state_.has_position()); - ASSERT_FALSE(desired_state_.has_velocity()); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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; @@ -134,11 +134,11 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) 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); - ASSERT_TRUE(desired_state_.has_position()); - ASSERT_FALSE(desired_state_.has_velocity()); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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(); @@ -150,11 +150,11 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) 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); - ASSERT_TRUE(desired_state_.has_position()); - ASSERT_FALSE(desired_state_.has_velocity()); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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) @@ -178,40 +178,40 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) rclcpp::Duration period(1, 0); // 1 second desired_state_.velocity = 2.0; - ASSERT_FALSE(desired_state_.has_position()); - ASSERT_TRUE(desired_state_.has_velocity()); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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()); // 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)); - ASSERT_FALSE(desired_state_.has_position()); - ASSERT_TRUE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); EXPECT_NEAR(desired_state_.velocity.value(), limits.max_velocity, COMMON_THRESHOLD); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); desired_state_.velocity = -5.0; ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); - ASSERT_FALSE(desired_state_.has_position()); - ASSERT_TRUE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); EXPECT_NEAR(desired_state_.velocity.value(), -limits.max_velocity, COMMON_THRESHOLD); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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_.velocity = 1.0; ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); - ASSERT_FALSE(desired_state_.has_position()); - ASSERT_TRUE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); EXPECT_NEAR(desired_state_.velocity.value(), 1.0, COMMON_THRESHOLD); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + 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 @@ -226,12 +226,12 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) actual_state_.position = actual_position; desired_state_.velocity = desired_velocity; ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); - ASSERT_FALSE(desired_state_.has_position()); - ASSERT_TRUE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); - ASSERT_FALSE(desired_state_.has_acceleration()); - ASSERT_FALSE(desired_state_.has_effort()); - ASSERT_FALSE(desired_state_.has_jerk()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); }; test_limit_enforcing(4.5, 5.0, 0.5, true); test_limit_enforcing(4.8, 5.0, 0.2, true); From 5b2c0b4c4a96105a85f70897cc64309611e2eea0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 10:41:47 +0200 Subject: [PATCH 28/71] Use lambdas for better testing --- .../test/test_joint_range_limiter.cpp | 66 +++++++++---------- 1 file changed, 32 insertions(+), 34 deletions(-) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 2f0a7e6f09..559e7fd168 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -15,6 +15,7 @@ /// \author Sai Kishor Kothakota #include "test_joint_range_limiter.hpp" +#include TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) { @@ -184,46 +185,27 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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_FALSE(desired_state_.has_position()); - EXPECT_TRUE(desired_state_.has_velocity()); - EXPECT_NEAR(desired_state_.velocity.value(), limits.max_velocity, COMMON_THRESHOLD); - EXPECT_FALSE(desired_state_.has_acceleration()); - EXPECT_FALSE(desired_state_.has_effort()); - EXPECT_FALSE(desired_state_.has_jerk()); - - desired_state_.velocity = -5.0; - ASSERT_TRUE(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(), -limits.max_velocity, COMMON_THRESHOLD); - 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_.velocity = 1.0; - ASSERT_FALSE(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(), 1.0, COMMON_THRESHOLD); - 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 = - [&](double actual_position, double desired_velocity, double expected_velocity, bool is_clamped) + 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(actual_position) + + "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)); - actual_state_.position = actual_position; + 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()); @@ -233,6 +215,22 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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); + 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); From 0dde6a9293c5363b2b0f5ac6f1dc2c6f4a9cf462 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 12:08:49 +0200 Subject: [PATCH 29/71] If the joint is outside the position range, then don't let the joint move --- joint_limits/src/joint_range_limiter.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 204d0fdc58..423c70a295 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -62,8 +62,8 @@ std::pair compute_position_limits( } std::pair compute_velocity_limits( - joint_limits::JointLimits limits, const std::optional & act_pos, - const std::optional & prev_command_vel, double dt) + const std::string & joint_name, 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(); @@ -74,6 +74,15 @@ std::pair compute_velocity_limits( 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()) { @@ -216,8 +225,8 @@ bool JointSaturationLimiter::on_enforce if (desired.has_velocity()) { - const auto limits = - compute_velocity_limits(joint_limits, actual.position, prev_command_.velocity, dt_seconds); + const auto limits = compute_velocity_limits( + joint_name, joint_limits, actual.position, prev_command_.velocity, dt_seconds); limits_enforced = limits_enforced || is_limited(desired.velocity.value(), limits.first, limits.second); desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); From 80308b3b743202865c06ea46b463dc2834901bdc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 12:09:47 +0200 Subject: [PATCH 30/71] extend tests of acceleration limits also with and without actual position value --- .../test/test_joint_range_limiter.cpp | 41 ++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 559e7fd168..f891328e28 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -217,7 +217,6 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) }; // 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); @@ -231,10 +230,15 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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); @@ -242,6 +246,41 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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); + + // 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 not 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); + } } // TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) From 92d153ebf61c9e31bc91c785fa77ca1975559954 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 12:23:46 +0200 Subject: [PATCH 31/71] Add tests for the case of actuators with actual position state and acceleration limits --- .../test/test_joint_range_limiter.cpp | 30 +++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index f891328e28..fdaeb870c7 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -249,7 +249,9 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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, -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; @@ -257,7 +259,7 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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 not saturated by the acceleration limits so in succeeding call it + // 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 : @@ -281,6 +283,30 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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, when_no_posstate_expect_enforce_false) From a2d72584319cae059e940e0ffa5939287dd1d61c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 12:24:31 +0200 Subject: [PATCH 32/71] remove unused test cases --- .../test/test_joint_range_limiter.cpp | 444 ------------------ 1 file changed, 444 deletions(-) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index fdaeb870c7..45af6ca88f 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -309,450 +309,6 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) test_limit_enforcing(6.0, -1.0, 0.0, true); } -// TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// joint_limiter_->configure(last_commanded_state_); - -// rclcpp::Duration period(1, 0); // 1 second -// // test no position interface -// current_joint_states_.positions.clear(); -// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // also fail if out of limits -// desired_joint_states_.positions[0] = 20.0; -// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// joint_limiter_->configure(last_commanded_state_); - -// rclcpp::Duration period(1, 0); // 1 second -// // test no vel interface -// current_joint_states_.velocities.clear(); -// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); -// // also fail if out of limits -// desired_joint_states_.positions[0] = 20.0; -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(1.0, 0.0); // 1 second -// // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 - -// // within limits -// desired_joint_states_.positions[0] = 1.0; -// desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well -// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if no limits applied -// CHECK_STATE_SINGLE_JOINT( -// desired_joint_states_, 0, -// 1.0, // pos unchanged -// 1.0, // vel unchanged -// 1.0 // acc = vel / 1.0 -// ); -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_with_acc) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(1.0, 0.0); // 1 second -// // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 - -// // within limits -// desired_joint_states_.positions[0] = 1.0; -// desired_joint_states_.velocities[0] = 1.5; // valid pos derivative as well -// desired_joint_states_.accelerations[0] = 2.9; // valid pos derivative as well -// ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if no limits applied -// CHECK_STATE_SINGLE_JOINT( -// desired_joint_states_, 0, -// 1.0, // pos unchanged -// 1.5, // vel unchanged -// 2.9 // acc = vel / 1.0 -// ); -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(1.0, 0.0); // 1 second - -// // pos/vel cmd ifs -// current_joint_states_.positions[0] = -2.0; -// desired_joint_states_.positions[0] = 1.0; -// // desired velocity exceeds although correctly computed from pos derivative -// desired_joint_states_.velocities[0] = 3.0; -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if limits applied -// CHECK_STATE_SINGLE_JOINT( -// desired_joint_states_, 0, -// 0.0, // pos = pos + max_vel * dt -// 2.0, // vel limited to max_vel -// 2.0 / 1.0 // acc set to vel change/DT -// ); - -// // check opposite velocity direction (sign copy) -// current_joint_states_.positions[0] = 2.0; -// desired_joint_states_.positions[0] = -1.0; -// // desired velocity exceeds although correctly computed from pos derivative -// desired_joint_states_.velocities[0] = -3.0; -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if vel and acc limits applied -// CHECK_STATE_SINGLE_JOINT( -// desired_joint_states_, 0, -// 0.0, // pos = pos - max_vel * dt -// -2.0, // vel limited to -max_vel -// -2.0 / 1.0 // acc set to vel change/DT -// ); -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(1.0, 0.0); // 1 second - -// // set current velocity close to limits to not be blocked by max acc to reach max -// current_joint_states_.velocities[0] = 1.9; -// // desired pos leads to vel exceeded (4.0 / sec instead of 2.0) -// desired_joint_states_.positions[0] = 4.0; -// // no vel cmd (will lead to internal computation of velocity) -// desired_joint_states_.velocities.clear(); -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if pos and acc limits applied -// ASSERT_EQ(desired_joint_states_.positions[0], 2.0); // pos limited to max_vel * DT -// // no vel cmd ifs -// ASSERT_EQ( -// desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT - -// // check opposite position and direction -// current_joint_states_.positions[0] = 0.0; -// current_joint_states_.velocities[0] = -1.9; -// desired_joint_states_.positions[0] = -4.0; -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if pos and acc limits applied -// ASSERT_EQ(desired_joint_states_.positions[0], -2.0); // pos limited to -max_vel * DT -// // no vel cmd ifs -// ASSERT_EQ( -// desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(1.0, 0.0); // 1 second - -// // vel cmd ifs -// current_joint_states_.positions[0] = -2.0; -// // set current velocity close to limits to not be blocked by max acc to reach max -// current_joint_states_.velocities[0] = 1.9; -// // no pos cmd -// desired_joint_states_.positions.clear(); -// // desired velocity exceeds -// desired_joint_states_.velocities[0] = 3.0; - -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if vel and acc limits applied -// ASSERT_EQ(desired_joint_states_.velocities[0], 2.0); // vel limited to max_vel -// // no vel cmd ifs -// ASSERT_EQ( -// desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT - -// // check opposite velocity direction -// current_joint_states_.positions[0] = 2.0; -// // set current velocity close to limits to not be blocked by max acc to reach max -// current_joint_states_.velocities[0] = -1.9; -// // desired velocity exceeds -// desired_joint_states_.velocities[0] = -3.0; - -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if vel and acc limits applied -// ASSERT_EQ(desired_joint_states_.velocities[0], -2.0); // vel limited to -max_vel -// // no vel cmd ifs -// ASSERT_EQ( -// desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(1.0, 0.0); // 1 second - -// // desired pos exceeds -// current_joint_states_.positions[0] = 5.0; -// desired_joint_states_.positions[0] = 20.0; -// // no velocity interface -// desired_joint_states_.velocities.clear(); -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if pos limits applied -// ASSERT_EQ( -// desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit) -// // no vel cmd ifs -// ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// // using 0.05 because 1.0 sec invalidates the "small dt integration" -// rclcpp::Duration period(0, 50000000); // 0.05 second - -// // close to pos limit should reduce velocity and stop -// current_joint_states_.positions[0] = 4.851; -// current_joint_states_.velocities[0] = 1.5; -// desired_joint_states_.positions[0] = 4.926; -// desired_joint_states_.velocities[0] = 1.5; - -// // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) -// std::vector expected_ret = {true, true, true, false}; -// for (auto i = 0u; i < 4; ++i) -// { -// auto previous_vel_request = desired_joint_states_.velocities[0]; -// // expect limits applied until the end stop -// ASSERT_EQ( -// joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period), -// expected_ret[i]); - -// ASSERT_LE( -// desired_joint_states_.velocities[0], -// previous_vel_request); // vel adapted to reach end-stop should be decreasing -// // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit -// // hence no max deceleration anymore... -// ASSERT_LT( -// desired_joint_states_.positions[0], -// 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits -// ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate - -// Integrate(period.seconds()); - -// ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit -// } -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(0, 50000000); - -// // desired acceleration exceeds -// current_joint_states_.positions[0] = 0.1; -// current_joint_states_.velocities[0] = 0.1; -// desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity -// desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if limits applied -// CHECK_STATE_SINGLE_JOINT( -// desired_joint_states_, 0, -// 0.11125, // pos = double integration from max acc with current state -// 0.35, // vel limited to vel + max acc * dt -// 5.0 // acc max acc -// ); -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(0, 50000000); - -// // desired acceleration exceeds -// current_joint_states_.positions[0] = 0.1; -// current_joint_states_.velocities[0] = 0.1; -// desired_joint_states_.positions[0] = -// 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc -// desired_joint_states_.velocities.clear(); -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if pos and acc limits applied -// ASSERT_NEAR( -// desired_joint_states_.positions[0], 0.111250, -// COMMON_THRESHOLD); // pos = double integration from max acc with current state -// // no vel cmd ifs -// ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(0, 50000000); - -// // desired acceleration exceeds -// current_joint_states_.positions[0] = 0.1; -// current_joint_states_.velocities[0] = 0.1; -// desired_joint_states_.positions.clear(); // = 0.125; -// desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if pos and acc limits applied -// // no pos cmd ifs -// ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt -// ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforced) -// { -// SetupNode("joint_saturation_limiter"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(0, 50000000); - -// // desired deceleration exceeds -// current_joint_states_.positions[0] = 0.3; -// current_joint_states_.velocities[0] = 0.5; -// desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity -// desired_joint_states_.velocities[0] = 0.1; // leads to acc < -max dec - -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if vel and acc limits applied -// CHECK_STATE_SINGLE_JOINT( -// desired_joint_states_, 0, -// 0.315625, // pos = double integration from max dec with current state -// 0.125, // vel limited by vel - max dec * dt -// -7.5 // acc limited by -max dec -// ); -// } -// } - -// TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) -// { -// SetupNode("joint_saturation_limiter_nodeclimit"); -// Load(); - -// if (joint_limiter_) -// { -// Init(); -// Configure(); - -// rclcpp::Duration period(0, 50000000); - -// // desired deceleration exceeds -// current_joint_states_.positions[0] = 1.0; -// current_joint_states_.velocities[0] = 1.0; -// desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased -// velocity desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc -// ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); - -// // check if vel and acc limits applied -// CHECK_STATE_SINGLE_JOINT( -// desired_joint_states_, 0, -// 1.04375, // pos = double integration from max acc with current state -// 0.75, // vel limited by vel-max acc * dt -// -5.0 // acc limited to -max acc -// ); -// } -// } - int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From c1e29c9601315be768a91aec81532f00b20e8fe0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 13:15:17 +0200 Subject: [PATCH 33/71] pass by const reference and parse the optional actual position and velocity --- joint_limits/src/joint_range_limiter.cpp | 25 ++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 423c70a295..d7e2a73982 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -24,7 +24,7 @@ bool is_limited(double value, double min, double max) { return value < min || value > max; } std::pair compute_position_limits( - joint_limits::JointLimits limits, double prev_command_pos, double dt) + const joint_limits::JointLimits & limits, double prev_command_pos, double dt) { std::pair pos_limits({limits.min_position, limits.max_position}); if (limits.has_velocity_limits) @@ -37,7 +37,7 @@ std::pair compute_position_limits( } std::pair compute_position_limits( - joint_limits::JointLimits limits, joint_limits::SoftJointLimits soft_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}); @@ -62,7 +62,7 @@ std::pair compute_position_limits( } std::pair compute_velocity_limits( - const std::string & joint_name, joint_limits::JointLimits 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 = @@ -94,29 +94,30 @@ std::pair compute_velocity_limits( } std::pair compute_effort_limits( - joint_limits::JointLimits limits, double act_pos, double act_vel, double /*dt*/) + 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) + if (limits.has_position_limits && act_pos.has_value() && act_vel.has_value()) { - if ((act_pos <= limits.min_position) && (act_vel <= 0.0)) + if ((act_pos.value() <= limits.min_position) && (act_vel.value() <= 0.0)) { eff_limits.first = 0.0; } - else if ((act_pos >= limits.max_position) && (act_vel >= 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) + if (limits.has_velocity_limits && act_vel.has_value()) { - if (act_vel < -limits.max_velocity) + if (act_vel.value() < -limits.max_velocity) { eff_limits.first = 0.0; } - else if (act_vel > limits.max_velocity) + else if (act_vel.value() > limits.max_velocity) { eff_limits.second = 0.0; } @@ -234,8 +235,8 @@ bool JointSaturationLimiter::on_enforce if (desired.has_effort()) { - const auto limits = compute_effort_limits( - joint_limits, actual.position.value(), actual.velocity.value(), dt_seconds); + const auto limits = + compute_effort_limits(joint_limits, actual.position, actual.velocity, dt_seconds); limits_enforced = limits_enforced || is_limited(desired.effort.value(), limits.first, limits.second); desired.effort = std::clamp(desired.effort.value(), limits.first, limits.second); From 3746584a144198db996c043046a141e630705b3e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 13:15:39 +0200 Subject: [PATCH 34/71] Added tests for the effort case --- .../test/test_joint_range_limiter.cpp | 115 ++++++++++++++++++ 1 file changed, 115 insertions(+) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 45af6ca88f..e47cd343b8 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -309,6 +309,121 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From 1eb5bad648970c2f67b4678fb5fd78e2af12f968 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 18:05:39 +0200 Subject: [PATCH 35/71] better conditioning for the acceleration limits enforcement --- joint_limits/src/joint_range_limiter.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index d7e2a73982..e846e20835 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -262,8 +262,10 @@ bool JointSaturationLimiter::on_enforce double desired_acc = desired.acceleration.value(); if ( joint_limits.has_deceleration_limits && - ((desired.acceleration.value() < 0 && actual.velocity.value() > 0) || - (desired.acceleration.value() > 0 && actual.velocity.value() < 0))) + ((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 = From 5c861001df38ef80806b7b96d47a5bca7ba85061 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 18:05:59 +0200 Subject: [PATCH 36/71] Added tests for the desired acceleration case --- .../test/test_joint_range_limiter.cpp | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index e47cd343b8..7de4adb9fb 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -424,6 +424,95 @@ TEST_F(JointSaturationLimiterTest, check_desired_effort_only_cases) 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); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From 90cd75ed4158dcf12b65c7852f31bd5d29ad7c37 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 18:16:07 +0200 Subject: [PATCH 37/71] Add tests for the desired jerk test cases --- .../test/test_joint_range_limiter.cpp | 41 +++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 7de4adb9fb..9982721739 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -513,6 +513,47 @@ TEST_F(JointSaturationLimiterTest, check_desired_acceleration_only_cases) 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); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From 71d311d0026fa223f03617025fa083d3fe0ca208 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 23:35:43 +0200 Subject: [PATCH 38/71] consider also the acceleration limits when computing the position limiting --- joint_limits/src/joint_range_limiter.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index e846e20835..22cd78e550 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -29,7 +29,10 @@ std::pair compute_position_limits( std::pair pos_limits({limits.min_position, limits.max_position}); if (limits.has_velocity_limits) { - const double delta_pos = limits.max_velocity * dt; + const double delta_vel = + limits.has_acceleration_limits ? 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 - delta_pos, pos_limits.first); pos_limits.second = std::min(prev_command_pos + delta_pos, pos_limits.second); } From ffe9aeb17d1008e4dfd1e449a4892f8d33ed2129 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 23:36:19 +0200 Subject: [PATCH 39/71] Fix minor bug in the limit enforcement --- joint_limits/src/joint_range_limiter.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 22cd78e550..40b7aab7a4 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -232,7 +232,7 @@ bool JointSaturationLimiter::on_enforce const auto limits = compute_velocity_limits( joint_name, joint_limits, actual.position, prev_command_.velocity, dt_seconds); limits_enforced = - limits_enforced || is_limited(desired.velocity.value(), limits.first, limits.second); + is_limited(desired.velocity.value(), limits.first, limits.second) || limits_enforced; desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); } @@ -241,7 +241,7 @@ bool JointSaturationLimiter::on_enforce const auto limits = compute_effort_limits(joint_limits, actual.position, actual.velocity, dt_seconds); limits_enforced = - limits_enforced || is_limited(desired.effort.value(), limits.first, limits.second); + is_limited(desired.effort.value(), limits.first, limits.second) || limits_enforced; desired.effort = std::clamp(desired.effort.value(), limits.first, limits.second); } @@ -280,7 +280,7 @@ bool JointSaturationLimiter::on_enforce if (joint_limits.has_acceleration_limits) { limits_enforced = - limits_enforced || apply_acc_or_dec_limit(joint_limits.max_acceleration, desired_acc); + apply_acc_or_dec_limit(joint_limits.max_acceleration, desired_acc) || limits_enforced; } } desired.acceleration = desired_acc; @@ -289,8 +289,8 @@ bool JointSaturationLimiter::on_enforce if (desired.has_jerk()) { limits_enforced = - limits_enforced || - is_limited(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk); + 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); } From cd60d881acd439271bf1d144068eccfba01cbf5d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 19 Apr 2024 00:23:30 +0200 Subject: [PATCH 40/71] better computation of the position limits --- joint_limits/src/joint_range_limiter.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 40b7aab7a4..872416562c 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -24,17 +24,20 @@ bool is_limited(double value, double min, double max) { return value < min || value > max; } std::pair compute_position_limits( - const joint_limits::JointLimits & limits, double prev_command_pos, double dt) + 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 delta_vel = - limits.has_acceleration_limits ? limits.max_acceleration * dt : limits.max_velocity; + 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 - delta_pos, pos_limits.first); - pos_limits.second = std::min(prev_command_pos + delta_pos, pos_limits.second); + 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; } @@ -222,7 +225,7 @@ bool JointSaturationLimiter::on_enforce if (desired.has_position()) { const auto limits = - compute_position_limits(joint_limits, prev_command_.position.value(), dt_seconds); + 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); } From bde797ae102fbc3a71267b26c4cc0bec469a140e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 19 Apr 2024 00:23:46 +0200 Subject: [PATCH 41/71] better combined desired references test --- .../test/test_joint_range_limiter.cpp | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 9982721739..b010b4561c 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -554,6 +554,95 @@ TEST_F(JointSaturationLimiterTest, check_desired_jerk_only_cases) 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); From 1d2faa959616e9626b5adfb268298bb42789531b Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Mon, 22 Apr 2024 17:55:51 +0200 Subject: [PATCH 42/71] Remove LimitsType template from JointLimiterInterface --- .../joint_limits/joint_limiter_interface.hpp | 12 +++++++----- .../joint_limits/joint_saturation_limiter.hpp | 15 ++++++++------- joint_limits/src/joint_range_limiter.cpp | 10 ++++------ joint_limits/src/joint_saturation_limiter.cpp | 8 +++----- joint_limits/test/test_joint_range_limiter.hpp | 6 ++---- .../test/test_joint_saturation_limiter.hpp | 7 +++---- 6 files changed, 27 insertions(+), 31 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 601013e611..32253cf4e6 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 "joint_limits/visibility_control.h" #include "rclcpp/node.hpp" @@ -30,7 +31,7 @@ namespace joint_limits { -template +template class JointLimiterInterface { public: @@ -99,7 +100,7 @@ class JointLimiterInterface rcl_interfaces::msg::SetParametersResult set_parameters_result; set_parameters_result.successful = true; - std::vector updated_joint_limits = joint_limits_; + std::vector updated_joint_limits = joint_limits_; bool changed = false; for (size_t i = 0; i < number_of_joints_; ++i) @@ -135,7 +136,8 @@ class JointLimiterInterface * 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 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) { @@ -253,13 +255,13 @@ class JointLimiterInterface size_t number_of_joints_; std::vector joint_names_; - std::vector joint_limits_; + std::vector joint_limits_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_; private: rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; - realtime_tools::RealtimeBuffer> updated_limits_; + realtime_tools::RealtimeBuffer> updated_limits_; }; } // namespace joint_limits diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 46dab960b0..3dc00bca09 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -20,6 +20,7 @@ #include #include +#include "joint_limiter_interface.hpp" #include "joint_limits/joint_limiter_interface.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -33,8 +34,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 */ @@ -76,15 +77,15 @@ class JointSaturationLimiter : public JointLimiterInterface -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/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 872416562c..0b63c2ed71 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -138,7 +138,7 @@ namespace joint_limits { template <> -bool JointSaturationLimiter::on_init() +bool JointSaturationLimiter::on_init() { const bool result = (number_of_joints_ == 1); if (!result && has_logging_interface()) @@ -154,7 +154,7 @@ bool JointSaturationLimiter::on_init() } template <> -bool JointSaturationLimiter::on_enforce( +bool JointSaturationLimiter::on_enforce( JointControlInterfacesData & actual, JointControlInterfacesData & desired, const rclcpp::Duration & dt) { @@ -308,10 +308,8 @@ bool JointSaturationLimiter::on_enforce // 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> +typedef joint_limits::JointSaturationLimiter JointInterfacesSaturationLimiter; -typedef joint_limits::JointLimiterInterface< - joint_limits::JointLimits, joint_limits::JointControlInterfacesData> +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 317a6b7515..3097d0da26 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -26,7 +26,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) { @@ -433,11 +433,9 @@ bool JointSaturationLimiter int_map; -typedef joint_limits::JointSaturationLimiter< - joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint> +typedef joint_limits::JointSaturationLimiter JointTrajectoryPointSaturationLimiter; -typedef joint_limits::JointLimiterInterface< - joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint> +typedef joint_limits::JointLimiterInterface JointTrajectoryPointLimiterInterfaceBase; PLUGINLIB_EXPORT_CLASS( JointTrajectoryPointSaturationLimiter, JointTrajectoryPointLimiterInterfaceBase) diff --git a/joint_limits/test/test_joint_range_limiter.hpp b/joint_limits/test/test_joint_range_limiter.hpp index eadbf08a6b..923e675bf7 100644 --- a/joint_limits/test/test_joint_range_limiter.hpp +++ b/joint_limits/test/test_joint_range_limiter.hpp @@ -29,8 +29,7 @@ const double COMMON_THRESHOLD = 1.0e-6; -using JointLimiter = joint_limits::JointLimiterInterface< - joint_limits::JointLimits, joint_limits::JointControlInterfacesData>; +using JointLimiter = joint_limits::JointLimiterInterface; class JointSaturationLimiterTest : public ::testing::Test { @@ -99,8 +98,7 @@ class JointSaturationLimiterTest : public ::testing::Test : joint_limiter_type_("joint_limits/JointInterfacesSaturationLimiter"), joint_limiter_loader_( "joint_limits", - "joint_limits::JointLimiterInterface") + "joint_limits::JointLimiterInterface") { } diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp index 7ab3474ec6..50b419d876 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -33,8 +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< - joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint>; +using JointLimiter = + joint_limits::JointLimiterInterface; class JointSaturationLimiterTest : public ::testing::Test { @@ -85,8 +85,7 @@ class JointSaturationLimiterTest : public ::testing::Test : joint_limiter_type_("joint_limits/JointTrajectoryPointSaturationLimiter"), joint_limiter_loader_( "joint_limits", - "joint_limits::JointLimiterInterface") + "joint_limits::JointLimiterInterface") { } From 46457733223cf1ac51ed3e2cf138aa07de188a01 Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Mon, 22 Apr 2024 18:03:00 +0200 Subject: [PATCH 43/71] Add SoftJointLimits in JointLimiterInterface --- joint_limits/include/joint_limits/joint_limiter_interface.hpp | 3 +++ joint_limits/test/test_joint_range_limiter.hpp | 3 ++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 32253cf4e6..75bf36bf48 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -138,12 +138,14 @@ class JointLimiterInterface JOINT_LIMITS_PUBLIC 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_); @@ -256,6 +258,7 @@ class JointLimiterInterface size_t number_of_joints_; std::vector joint_names_; 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_; diff --git a/joint_limits/test/test_joint_range_limiter.hpp b/joint_limits/test/test_joint_range_limiter.hpp index 923e675bf7..b8e58b991e 100644 --- a/joint_limits/test/test_joint_range_limiter.hpp +++ b/joint_limits/test/test_joint_range_limiter.hpp @@ -81,7 +81,8 @@ class JointSaturationLimiterTest : public ::testing::Test desired_state_ = last_commanded_state_; actual_state_ = last_commanded_state_; return joint_limiter_->init( - joint_names_, {limits}, nullptr, node_->get_node_logging_interface()); + joint_names_, {limits}, std::vector(), nullptr, + node_->get_node_logging_interface()); } bool Configure() { return joint_limiter_->configure(last_commanded_state_); } From 3192bc66e927cea4606fd677e9ca1ffe8a29efbb Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Thu, 25 Apr 2024 10:48:56 +0200 Subject: [PATCH 44/71] Move joint limits computation in a separate library --- joint_limits/CMakeLists.txt | 11 ++ .../joint_limits/joint_limits_helpers.hpp | 48 ++++++ joint_limits/src/joint_limits_helpers.cpp | 130 +++++++++++++++ joint_limits/src/joint_range_limiter.cpp | 156 +----------------- 4 files changed, 195 insertions(+), 150 deletions(-) create mode 100644 joint_limits/include/joint_limits/joint_limits_helpers.hpp create mode 100644 joint_limits/src/joint_limits_helpers.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 2a450328cc..cd9eac1b81 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -43,6 +43,16 @@ ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_ # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDING_DLL") +add_library(joint_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 @@ -114,6 +124,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/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp new file mode 100644 index 0000000000..fa2804c105 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -0,0 +1,48 @@ +// 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 "joint_limits/joint_limits.hpp" +#include + +namespace joint_limits +{ + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; + +bool is_limited(double value, double min, double 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 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); + +std::pair compute_effort_limits( + const joint_limits::JointLimits & limits, const std::optional & act_pos, + const std::optional & act_vel, double /*dt*/); + +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/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp new file mode 100644 index 0000000000..c9014a2a76 --- /dev/null +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -0,0 +1,130 @@ +// 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 "rclcpp/logging.hpp" +#include "joint_limits/joint_limits_helpers.hpp" +#include +#include + +namespace joint_limits +{ + +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_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; +} + +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 > 0) || + (desired_acceleration > 0 && actual_velocity && actual_velocity < 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 index 0b63c2ed71..b922fceef4 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -20,119 +20,7 @@ #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; +#include "joint_limits/joint_limits_helpers.hpp" namespace joint_limits { @@ -250,43 +138,11 @@ bool JointSaturationLimiter::on_enforce( 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; + 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()) From c9f36b009eb8ee0a0d44702f44fc5416f69a804e Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Thu, 25 Apr 2024 10:50:50 +0200 Subject: [PATCH 45/71] Implement SoftJointLimiter class --- joint_limits/CMakeLists.txt | 1 + .../joint_limits/soft_joint_limiter.hpp | 97 ++++++++ joint_limits/src/soft_joint_limiter.cpp | 215 ++++++++++++++++++ 3 files changed, 313 insertions(+) create mode 100644 joint_limits/include/joint_limits/soft_joint_limiter.hpp create mode 100644 joint_limits/src/soft_joint_limiter.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index cd9eac1b81..c236575906 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -57,6 +57,7 @@ ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEP add_library(joint_saturation_limiter SHARED src/joint_saturation_limiter.cpp src/joint_range_limiter.cpp + src/soft_joint_limiter.cpp ) target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) target_include_directories(joint_saturation_limiter PUBLIC diff --git a/joint_limits/include/joint_limits/soft_joint_limiter.hpp b/joint_limits/include/joint_limits/soft_joint_limiter.hpp new file mode 100644 index 0000000000..94627df157 --- /dev/null +++ b/joint_limits/include/joint_limits/soft_joint_limiter.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2023, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adrià Roig Moreno + +#ifndef JOINT_LIMITS__SOFT_JOINT_LIMITER_HPP +#define JOINT_LIMITS__SOFT_JOINT_LIMITER_HPP + +#include +#include +#include +#include + +#include "joint_limiter_interface.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" + +namespace joint_limits +{ +/** + * Joint Saturation Limiter limits joints' position, velocity and acceleration by clamping values + * to its minimal and maximal allowed values. Since the position, velocity and accelerations are + * variables in physical relation, it might be that some values are limited lower then specified + * limit. For example, if a joint is close to its position limit, velocity and acceleration will be + * reduced accordingly. + */ +template +class SoftJointLimiter : public JointLimiterInterface +{ +public: + /** \brief Constructor */ + JOINT_LIMITS_PUBLIC SoftJointLimiter(); + + /** \brief Destructor */ + JOINT_LIMITS_PUBLIC ~SoftJointLimiter(); + + JOINT_LIMITS_PUBLIC bool on_init() override { return true; } + + JOINT_LIMITS_PUBLIC bool on_configure( + const JointLimitsStateDataType & current_joint_states) override + { + prev_command_ = current_joint_states; + return true; + } + + /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. + * Class implements this method accepting following data types: + * - trajectory_msgs::msg::JointTrajectoryPoint: limiting position, velocity and acceleration; + * + * Implementation of saturation approach for joints with position, velocity or acceleration limits + * and values. First, position limits are checked to adjust desired velocity accordingly, then + * velocity and finally acceleration. + * The method support partial existence of limits, e.g., missing position limits for continuous + * joins. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC bool on_enforce( + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override; + +private: + rclcpp::Clock::SharedPtr clock_; + JointLimitsStateDataType prev_command_; +}; + +template +SoftJointLimiter::SoftJointLimiter() +: JointLimiterInterface() +{ + clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); +} + +template +SoftJointLimiter::~SoftJointLimiter() +{ +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__SOFT_JOINT_LIMITER_HPP diff --git a/joint_limits/src/soft_joint_limiter.cpp b/joint_limits/src/soft_joint_limiter.cpp new file mode 100644 index 0000000000..9226f29c8b --- /dev/null +++ b/joint_limits/src/soft_joint_limiter.cpp @@ -0,0 +1,215 @@ +// Copyright (c) 2023, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adrià Roig Moreno + +#include "joint_limits/soft_joint_limiter.hpp" +#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/joint_limits_helpers.hpp" + +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 SoftJointLimiter::on_init() +{ + const bool result = (number_of_joints_ == 1); + if (!result && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointInterfacesSoftLimiter: Expects the number of joints to be 1, but given : " + "%zu", + number_of_joints_); + } + prev_command_ = JointControlInterfacesData(); + return result; +} + +template <> +bool SoftJointLimiter::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]; + const auto 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; + } + } + + if(hard_limits.has_velocity_limits) + { + return false; + } + + double soft_min_vel = -hard_limits.max_velocity; + double soft_max_vel = hard_limits.max_velocity; + + if (hard_limits.has_position_limits) + { + soft_min_vel = std::clamp(-soft_joint_limits.k_position * (prev_command_.position.value() - soft_joint_limits.min_position), + -hard_limits.max_velocity, hard_limits.max_velocity); + + soft_max_vel = std::clamp(-soft_joint_limits.k_position * (prev_command_.position.value() - soft_joint_limits.max_position), + -hard_limits.max_velocity, hard_limits.max_velocity); + } + + if(desired.has_position()) + { + double pos_low = prev_command_.position.value() + soft_min_vel * dt_seconds; + double pos_high = prev_command_.position.value() + soft_max_vel * dt_seconds; + + if (hard_limits.has_position_limits) + { + const auto position_limits = + compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); + 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, actual.position, prev_command_.velocity, dt_seconds); + + if(hard_limits.has_acceleration_limits) + { + soft_min_vel = std::max(prev_command_.velocity.value() - hard_limits.max_acceleration * dt_seconds, soft_min_vel); + soft_max_vel = std::min(prev_command_.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() && hard_limits.has_effort_limits) + { + const auto effort_limits = + compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds); + + double soft_min_eff = std::clamp(-soft_joint_limits.k_velocity * (prev_command_.velocity.value() - soft_min_vel), + -hard_limits.max_effort, hard_limits.max_effort); + + double soft_max_eff = std::clamp(-soft_joint_limits.k_velocity * (prev_command_.velocity.value() - soft_max_vel), + -hard_limits.max_effort, hard_limits.max_effort); + + soft_min_vel = std::max(soft_min_eff, effort_limits.first); + soft_max_vel = 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); + } + + prev_command_ = desired; + + return limits_enforced; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +typedef joint_limits::SoftJointLimiter + JointInterfacesSoftLimiter; +typedef joint_limits::JointLimiterInterface + JointInterfacesLimiterInterfaceBase; +PLUGINLIB_EXPORT_CLASS( + JointInterfacesSoftLimiter, JointInterfacesLimiterInterfaceBase) + + From 1e5dbf0aa3c44fcf7a5de5e715cc577116d0e4ec Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Thu, 25 Apr 2024 10:58:07 +0200 Subject: [PATCH 46/71] Export JointInterfacesSoftLimiter in the joint_limiters.xml --- joint_limits/joint_limiters.xml | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index 72dccfdc29..6ba9f17f17 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -2,17 +2,24 @@ + base_class_type="joint_limits::JointLimiterInterface<trajectory_msgs::msg::JointTrajectoryPoint>"> Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. + base_class_type="joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>"> 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. + + From 02f57600875eb3f62e4c603a9def4c098ea2a7cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adri=C3=A0=20Roig?= Date: Thu, 25 Apr 2024 15:09:37 +0200 Subject: [PATCH 47/71] Fix linking problem in test_joint_range_limiter --- joint_limits/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index c236575906..092bf38e1a 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -64,6 +64,8 @@ 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}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -110,7 +112,7 @@ if(BUILD_TESTING) 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 + test_joint_range_limiter pluginlib rclcpp ) From 6f02f9f4ed41dae925a5e42968b799c2ba745e6e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adri=C3=A0=20Roig?= Date: Thu, 25 Apr 2024 16:15:57 +0200 Subject: [PATCH 48/71] Create test_soft_joint_limiter.cpp --- joint_limits/CMakeLists.txt | 9 + ...nge_limiter.hpp => test_joint_limiter.hpp} | 45 +- .../test/test_joint_range_limiter.cpp | 2 +- joint_limits/test/test_soft_joint_limiter.cpp | 658 ++++++++++++++++++ 4 files changed, 707 insertions(+), 7 deletions(-) rename joint_limits/test/{test_joint_range_limiter.hpp => test_joint_limiter.hpp} (80%) create mode 100644 joint_limits/test/test_soft_joint_limiter.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 092bf38e1a..ac8d16580a 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -117,6 +117,15 @@ if(BUILD_TESTING) rclcpp ) + ament_add_gtest(test_soft_joint_limiter test/test_soft_joint_limiter.cpp) + target_include_directories(test_soft_joint_limiter PRIVATE include) + target_link_libraries(test_soft_joint_limiter joint_limiter_interface) + ament_target_dependencies( + test_soft_joint_limiter + pluginlib + rclcpp + ) + endif() install( diff --git a/joint_limits/test/test_joint_range_limiter.hpp b/joint_limits/test/test_joint_limiter.hpp similarity index 80% rename from joint_limits/test/test_joint_range_limiter.hpp rename to joint_limits/test/test_joint_limiter.hpp index b8e58b991e..b25640c083 100644 --- a/joint_limits/test/test_joint_range_limiter.hpp +++ b/joint_limits/test/test_joint_limiter.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef TEST_JOINT_LIMITER_HPP_ +#define TEST_JOINT_LIMITER_HPP_ #include @@ -31,7 +31,7 @@ const double COMMON_THRESHOLD = 1.0e-6; using JointLimiter = joint_limits::JointLimiterInterface; -class JointSaturationLimiterTest : public ::testing::Test +class JointLimiterTest : public ::testing::Test { public: void SetUp() override @@ -95,14 +95,19 @@ class JointSaturationLimiterTest : public ::testing::Test actual_state_.velocity.value() + desired_state_.acceleration.value() * dt; } - JointSaturationLimiterTest() - : joint_limiter_type_("joint_limits/JointInterfacesSaturationLimiter"), + 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: @@ -119,4 +124,32 @@ class JointSaturationLimiterTest : public ::testing::Test joint_limits::JointControlInterfacesData actual_state_; }; -#endif // TEST_JOINT_RANGE_LIMITER_HPP_ +class JointSaturationLimiterTest : public JointLimiterTest +{ +public: + JointSaturationLimiterTest() + : JointLimiterTest("joint_limits/JointInterfacesSaturationLimiter") + { + } +}; + +class SoftJointLimiterTest : public JointLimiterTest +{ +public: + SoftJointLimiterTest() + : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") + { + } + + bool Init(const joint_limits::JointLimits & limits, + const joint_limits::SoftJointLimits &soft_limits, + const std::string & joint_name = "foo_joint") + { + soft_limits_ = soft_limits; + return JointLimiterTest::Init(limits, joint_name); + } +protected: + joint_limits::SoftJointLimits soft_limits_; +}; + +#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 index b010b4561c..49745a3347 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -14,7 +14,7 @@ /// \author Sai Kishor Kothakota -#include "test_joint_range_limiter.hpp" +#include "test_joint_limiter.hpp" #include TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) diff --git a/joint_limits/test/test_soft_joint_limiter.cpp b/joint_limits/test/test_soft_joint_limiter.cpp new file mode 100644 index 0000000000..554be03cd5 --- /dev/null +++ b/joint_limits/test/test_soft_joint_limiter.cpp @@ -0,0 +1,658 @@ +// 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 "test_joint_limiter.hpp" +#include + +/*TEST_F(SoftJointLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test SoftJointLimiter 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(SoftJointLimiterTest, 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(SoftJointLimiterTest, 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(SoftJointLimiterTest, 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; +// soft_limits.min_position = -3.0; +// soft_limits.max_position = 3.0; +// soft_limits.k_position = 10.0; +// soft_limits.k_velocity = 10.0; + 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()); + + // 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(SoftJointLimiterTest, 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(SoftJointLimiterTest, 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(SoftJointLimiterTest, 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(SoftJointLimiterTest, 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(SoftJointLimiterTest, 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; +} From c4ea119c3ee481183f044a6dff36a3fa98c7e7aa Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Mon, 29 Apr 2024 10:58:04 +0200 Subject: [PATCH 49/71] Fix typo in plugins definition --- joint_limits/joint_limiters.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml index 6ba9f17f17..f2a4b8e20c 100644 --- a/joint_limits/joint_limiters.xml +++ b/joint_limits/joint_limiters.xml @@ -15,7 +15,7 @@ Simple joint range limiter performing clamping between the parsed soft limits and the parsed joint limits. From 9f31c6a33801490f44a67c5931972a5d489b33ea Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Mon, 29 Apr 2024 10:59:21 +0200 Subject: [PATCH 50/71] Rm soft_joint_limiter.hpp --- .../joint_limits/soft_joint_limiter.hpp | 97 ------------------- 1 file changed, 97 deletions(-) delete mode 100644 joint_limits/include/joint_limits/soft_joint_limiter.hpp diff --git a/joint_limits/include/joint_limits/soft_joint_limiter.hpp b/joint_limits/include/joint_limits/soft_joint_limiter.hpp deleted file mode 100644 index 94627df157..0000000000 --- a/joint_limits/include/joint_limits/soft_joint_limiter.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (c) 2023, PickNik Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adrià Roig Moreno - -#ifndef JOINT_LIMITS__SOFT_JOINT_LIMITER_HPP -#define JOINT_LIMITS__SOFT_JOINT_LIMITER_HPP - -#include -#include -#include -#include - -#include "joint_limiter_interface.hpp" -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" - -namespace joint_limits -{ -/** - * Joint Saturation Limiter limits joints' position, velocity and acceleration by clamping values - * to its minimal and maximal allowed values. Since the position, velocity and accelerations are - * variables in physical relation, it might be that some values are limited lower then specified - * limit. For example, if a joint is close to its position limit, velocity and acceleration will be - * reduced accordingly. - */ -template -class SoftJointLimiter : public JointLimiterInterface -{ -public: - /** \brief Constructor */ - JOINT_LIMITS_PUBLIC SoftJointLimiter(); - - /** \brief Destructor */ - JOINT_LIMITS_PUBLIC ~SoftJointLimiter(); - - JOINT_LIMITS_PUBLIC bool on_init() override { return true; } - - JOINT_LIMITS_PUBLIC bool on_configure( - const JointLimitsStateDataType & current_joint_states) override - { - prev_command_ = current_joint_states; - return true; - } - - /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. - * Class implements this method accepting following data types: - * - trajectory_msgs::msg::JointTrajectoryPoint: limiting position, velocity and acceleration; - * - * Implementation of saturation approach for joints with position, velocity or acceleration limits - * and values. First, position limits are checked to adjust desired velocity accordingly, then - * velocity and finally acceleration. - * The method support partial existence of limits, e.g., missing position limits for continuous - * joins. - * - * \param[in] current_joint_states current joint states a robot is in. - * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. - * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. - * \returns true if limits are enforced, otherwise false. - */ - JOINT_LIMITS_PUBLIC bool on_enforce( - JointLimitsStateDataType & current_joint_states, - JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override; - -private: - rclcpp::Clock::SharedPtr clock_; - JointLimitsStateDataType prev_command_; -}; - -template -SoftJointLimiter::SoftJointLimiter() -: JointLimiterInterface() -{ - clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); -} - -template -SoftJointLimiter::~SoftJointLimiter() -{ -} - -} // namespace joint_limits - -#endif // JOINT_LIMITS__SOFT_JOINT_LIMITER_HPP From b58bd1041df7afec85dd81f52ee62f481836753e Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Mon, 29 Apr 2024 11:03:05 +0200 Subject: [PATCH 51/71] Derive SoiftJointLimiter from JointSaturationLimiter and enforce hard limtis --- .../joint_limits/joint_limits_helpers.hpp | 3 - .../joint_limits/joint_saturation_limiter.hpp | 3 +- joint_limits/src/joint_saturation_limiter.cpp | 1 + joint_limits/src/soft_joint_limiter.cpp | 109 ++++++++++++------ 4 files changed, 75 insertions(+), 41 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index fa2804c105..da0000a857 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -23,9 +23,6 @@ namespace joint_limits { -constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops -constexpr double VALUE_CONSIDERED_ZERO = 1e-10; - bool is_limited(double value, double min, double max); std::pair compute_position_limits( diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 3dc00bca09..fe619f9f06 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -20,7 +20,6 @@ #include #include -#include "joint_limiter_interface.hpp" #include "joint_limits/joint_limiter_interface.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -72,7 +71,7 @@ class JointSaturationLimiter : public JointLimiterInterface diff --git a/joint_limits/src/soft_joint_limiter.cpp b/joint_limits/src/soft_joint_limiter.cpp index 9226f29c8b..4258dc69d3 100644 --- a/joint_limits/src/soft_joint_limiter.cpp +++ b/joint_limits/src/soft_joint_limiter.cpp @@ -14,24 +14,28 @@ /// \author Adrià Roig Moreno -#include "joint_limits/soft_joint_limiter.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" #include "joint_limits/joint_limiter_struct.hpp" #include "joint_limits/joint_limits_helpers.hpp" +#include -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 SoftJointLimiter::on_init() + +class SoftJointLimiter : public JointSaturationLimiter +{ +public: + +bool on_init() { const bool result = (number_of_joints_ == 1); if (!result && has_logging_interface()) { RCLCPP_ERROR( node_logging_itf_->get_logger(), - "JointInterfacesSoftLimiter: Expects the number of joints to be 1, but given : " + "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : " "%zu", number_of_joints_); } @@ -39,8 +43,7 @@ bool SoftJointLimiter::on_init() return result; } -template <> -bool SoftJointLimiter::on_enforce( +bool on_enforce( JointControlInterfacesData & actual, JointControlInterfacesData & desired, const rclcpp::Duration & dt) { @@ -54,7 +57,12 @@ bool SoftJointLimiter::on_enforce( } const auto hard_limits = joint_limits_[0]; - const auto soft_joint_limits = soft_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()) @@ -109,36 +117,47 @@ bool SoftJointLimiter::on_enforce( } } - if(hard_limits.has_velocity_limits) - { - return false; - } - - double soft_min_vel = -hard_limits.max_velocity; - double soft_max_vel = hard_limits.max_velocity; + double soft_min_vel = -std::numeric_limits::infinity(); + double soft_max_vel = std::numeric_limits::infinity(); - if (hard_limits.has_position_limits) + if (hard_limits.has_velocity_limits) { - soft_min_vel = std::clamp(-soft_joint_limits.k_position * (prev_command_.position.value() - soft_joint_limits.min_position), - -hard_limits.max_velocity, hard_limits.max_velocity); + soft_min_vel = -hard_limits.max_velocity; + soft_max_vel = hard_limits.max_velocity; - soft_max_vel = std::clamp(-soft_joint_limits.k_position * (prev_command_.position.value() - soft_joint_limits.max_position), - -hard_limits.max_velocity, hard_limits.max_velocity); + if(hard_limits.has_position_limits && has_soft_limits(soft_joint_limits)) + { + soft_min_vel = std::clamp(-soft_joint_limits.k_position * (prev_command_.position.value() - soft_joint_limits.min_position), + -hard_limits.max_velocity, hard_limits.max_velocity); + + soft_max_vel = std::clamp(-soft_joint_limits.k_position * (prev_command_.position.value() - soft_joint_limits.max_position), + -hard_limits.max_velocity, hard_limits.max_velocity); + } } if(desired.has_position()) { - double pos_low = prev_command_.position.value() + soft_min_vel * dt_seconds; - double pos_high = prev_command_.position.value() + soft_max_vel * dt_seconds; + 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 (hard_limits.has_position_limits) + if(has_soft_position_limits(soft_joint_limits)) { - const auto position_limits = - compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); - pos_low = std::max(pos_low, position_limits.first); - pos_high = std::min(pos_high, position_limits.second); + pos_low = soft_joint_limits.min_position; + pos_high = soft_joint_limits.max_position; } + if(hard_limits.has_velocity_limits) + { + pos_low = prev_command_.position.value() + soft_min_vel * dt_seconds; + pos_high = prev_command_.position.value() + soft_max_vel * dt_seconds; + } + + 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); } @@ -161,20 +180,25 @@ bool SoftJointLimiter::on_enforce( desired.velocity = std::clamp(desired.velocity.value(), soft_min_vel, soft_max_vel); } - if(desired.has_effort() && hard_limits.has_effort_limits) + if(desired.has_effort()) { const auto effort_limits = compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds); - double soft_min_eff = std::clamp(-soft_joint_limits.k_velocity * (prev_command_.velocity.value() - soft_min_vel), - -hard_limits.max_effort, hard_limits.max_effort); + double soft_min_eff = effort_limits.first; + double soft_max_eff = effort_limits.second; - double soft_max_eff = std::clamp(-soft_joint_limits.k_velocity * (prev_command_.velocity.value() - soft_max_vel), - -hard_limits.max_effort, hard_limits.max_effort); + if(hard_limits.has_effort_limits && std::isfinite(soft_joint_limits.k_velocity)) + { + soft_min_eff = std::clamp(-soft_joint_limits.k_velocity * (prev_command_.velocity.value() - soft_min_vel), + -hard_limits.max_effort, hard_limits.max_effort); - soft_min_vel = std::max(soft_min_eff, effort_limits.first); - soft_max_vel = std::min(soft_max_eff, effort_limits.second); + soft_max_eff = std::clamp(-soft_joint_limits.k_velocity * (prev_command_.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); } @@ -201,12 +225,25 @@ bool SoftJointLimiter::on_enforce( return limits_enforced; } +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 #include "pluginlib/class_list_macros.hpp" -typedef joint_limits::SoftJointLimiter - JointInterfacesSoftLimiter; +typedef joint_limits::SoftJointLimiter JointInterfacesSoftLimiter; typedef joint_limits::JointLimiterInterface JointInterfacesLimiterInterfaceBase; PLUGINLIB_EXPORT_CLASS( From 00eb01d9161ef878cc70d386a93e9be933f28582 Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Mon, 29 Apr 2024 11:03:33 +0200 Subject: [PATCH 52/71] Test SofJointLimiter in position --- joint_limits/test/test_soft_joint_limiter.cpp | 64 +++++++++++++------ 1 file changed, 46 insertions(+), 18 deletions(-) diff --git a/joint_limits/test/test_soft_joint_limiter.cpp b/joint_limits/test/test_soft_joint_limiter.cpp index 554be03cd5..6c915a52fb 100644 --- a/joint_limits/test/test_soft_joint_limiter.cpp +++ b/joint_limits/test/test_soft_joint_limiter.cpp @@ -17,7 +17,7 @@ #include "test_joint_limiter.hpp" #include -/*TEST_F(SoftJointLimiterTest, when_loading_limiter_plugin_expect_loaded) +TEST_F(SoftJointLimiterTest, when_loading_limiter_plugin_expect_loaded) { // Test SoftJointLimiter loading ASSERT_NO_THROW( @@ -57,10 +57,6 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) limits.min_position = -M_PI; limits.max_position = M_PI; joint_limits::SoftJointLimits soft_limits; -// soft_limits.min_position = -3.0; -// soft_limits.max_position = 3.0; -// soft_limits.k_position = 10.0; -// soft_limits.k_velocity = 10.0; ASSERT_TRUE(Init(limits, soft_limits)); // no size check occurs (yet) so expect true ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); @@ -103,10 +99,37 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) EXPECT_FALSE(desired_state_.has_effort()); EXPECT_FALSE(desired_state_.has_jerk()); + 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); + 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); + 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); + 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); + 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(), soft_limits.min_position, COMMON_THRESHOLD); + // Now add the velocity limits limits.max_velocity = 1.0; limits.has_velocity_limits = true; - ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(Init(limits, soft_limits)); // Reset the desired and actual states desired_state_ = {}; actual_state_ = {}; @@ -132,7 +155,7 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) // 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)); + ASSERT_TRUE(Init(limits, soft_limits)); // Reset the desired and actual states desired_state_ = {}; actual_state_ = {}; @@ -148,7 +171,7 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) // Now test when there are no position limits, then the desired position is not saturated limits = joint_limits::JointLimits(); - ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(Init(limits, soft_limits)); // Reset the desired and actual states desired_state_ = {}; actual_state_ = {}; @@ -174,7 +197,8 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) limits.max_position = 5.0; limits.has_velocity_limits = true; limits.max_velocity = 1.0; - ASSERT_TRUE(Init(limits)); + 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_)); @@ -263,7 +287,7 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) 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)); + 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. @@ -294,7 +318,7 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) 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)); + 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. @@ -327,7 +351,8 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) limits.max_velocity = 1.0; limits.has_effort_limits = true; limits.max_effort = 200.0; - ASSERT_TRUE(Init(limits)); + 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 @@ -437,7 +462,8 @@ TEST_F(SoftJointLimiterTest, check_desired_acceleration_only_cases) joint_limits::JointLimits limits; limits.has_acceleration_limits = true; limits.max_acceleration = 0.5; - ASSERT_TRUE(Init(limits)); + 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 @@ -487,7 +513,7 @@ TEST_F(SoftJointLimiterTest, check_desired_acceleration_only_cases) 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)); + 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); @@ -526,7 +552,8 @@ TEST_F(SoftJointLimiterTest, check_desired_jerk_only_cases) joint_limits::JointLimits limits; limits.has_jerk_limits = true; limits.max_jerk = 0.5; - ASSERT_TRUE(Init(limits)); + 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 @@ -576,7 +603,8 @@ TEST_F(SoftJointLimiterTest, check_all_desired_references_limiting) limits.max_deceleration = 0.25; limits.has_jerk_limits = true; limits.max_jerk = 2.0; - ASSERT_TRUE(Init(limits)); + 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 @@ -635,7 +663,7 @@ TEST_F(SoftJointLimiterTest, check_all_desired_references_limiting) 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)); + 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); @@ -646,7 +674,7 @@ TEST_F(SoftJointLimiterTest, check_all_desired_references_limiting) 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) { From 2c6d1bd962d4af3242bf62204d513e0a5ee47f88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adri=C3=A0=20Roig?= Date: Mon, 29 Apr 2024 20:12:02 +0200 Subject: [PATCH 53/71] Solve issues in SoftJointLimiter when prev_command is not defined --- joint_limits/src/soft_joint_limiter.cpp | 56 ++++++++++++++++++++----- 1 file changed, 45 insertions(+), 11 deletions(-) diff --git a/joint_limits/src/soft_joint_limiter.cpp b/joint_limits/src/soft_joint_limiter.cpp index 4258dc69d3..e37b4743a5 100644 --- a/joint_limits/src/soft_joint_limiter.cpp +++ b/joint_limits/src/soft_joint_limiter.cpp @@ -119,6 +119,16 @@ bool on_enforce( double soft_min_vel = -std::numeric_limits::infinity(); double soft_max_vel = std::numeric_limits::infinity(); + double position = std::numeric_limits::infinity(); + + if(prev_command_.has_position() && std::isfinite(prev_command_.position.value())) + { + position = prev_command_.position.value(); + } + else if(actual.has_position()) + { + position = actual.position.value(); + } if (hard_limits.has_velocity_limits) { @@ -127,11 +137,18 @@ bool on_enforce( if(hard_limits.has_position_limits && has_soft_limits(soft_joint_limits)) { - soft_min_vel = std::clamp(-soft_joint_limits.k_position * (prev_command_.position.value() - soft_joint_limits.min_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 * (prev_command_.position.value() - soft_joint_limits.max_position), + 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 < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) + { + constexpr 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); + } } } @@ -151,10 +168,9 @@ bool on_enforce( if(hard_limits.has_velocity_limits) { - pos_low = prev_command_.position.value() + soft_min_vel * dt_seconds; - pos_high = prev_command_.position.value() + soft_max_vel * dt_seconds; + 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); @@ -167,10 +183,10 @@ bool on_enforce( const auto velocity_limits = compute_velocity_limits( joint_name, hard_limits, actual.position, prev_command_.velocity, dt_seconds); - if(hard_limits.has_acceleration_limits) + if(hard_limits.has_acceleration_limits && actual.has_velocity()) { - soft_min_vel = std::max(prev_command_.velocity.value() - hard_limits.max_acceleration * dt_seconds, soft_min_vel); - soft_max_vel = std::min(prev_command_.velocity.value() + hard_limits.max_acceleration * dt_seconds, soft_max_vel); + 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); @@ -188,17 +204,18 @@ bool on_enforce( 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)) + 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 * (prev_command_.velocity.value() - soft_min_vel), + 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 * (prev_command_.velocity.value() - soft_max_vel), + 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); } @@ -220,6 +237,23 @@ bool on_enforce( 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; + } + if(desired.has_velocity() && !std::isfinite(desired.velocity.value())) + { + desired.velocity = 0.0; + } + if(desired.has_acceleration() && !std::isfinite(desired.acceleration.value())) + { + desired.acceleration = 0.0; + } + if(desired.has_jerk() && !std::isfinite(desired.jerk.value())) + { + desired.jerk = 0.0; + } + prev_command_ = desired; return limits_enforced; From 764d68b5d4c8ebb9b1bb8ec6669b8f6f8c2b7a4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adri=C3=A0=20Roig?= Date: Mon, 29 Apr 2024 20:13:22 +0200 Subject: [PATCH 54/71] Test SoftJointLimiter in position and effort --- joint_limits/test/test_joint_limiter.hpp | 16 +- joint_limits/test/test_soft_joint_limiter.cpp | 202 +++++++++++++++++- 2 files changed, 211 insertions(+), 7 deletions(-) diff --git a/joint_limits/test/test_joint_limiter.hpp b/joint_limits/test/test_joint_limiter.hpp index b25640c083..5b909a0068 100644 --- a/joint_limits/test/test_joint_limiter.hpp +++ b/joint_limits/test/test_joint_limiter.hpp @@ -145,11 +145,19 @@ class SoftJointLimiterTest : public JointLimiterTest const joint_limits::SoftJointLimits &soft_limits, const std::string & joint_name = "foo_joint") { - soft_limits_ = soft_limits; - return JointLimiterTest::Init(limits, joint_name); + 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()); } -protected: - joint_limits::SoftJointLimits soft_limits_; }; #endif // TEST_JOINT_LIMITER_HPP_ diff --git a/joint_limits/test/test_soft_joint_limiter.cpp b/joint_limits/test/test_soft_joint_limiter.cpp index 6c915a52fb..c55c956e22 100644 --- a/joint_limits/test/test_soft_joint_limiter.cpp +++ b/joint_limits/test/test_soft_joint_limiter.cpp @@ -99,34 +99,41 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) 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)); @@ -147,14 +154,30 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) // 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 - // As per max velocity limit, it can only reach 0.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)); - EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + // 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_ = {}; @@ -169,8 +192,72 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) 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 + 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); + 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); + 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); + + soft_limits.k_position = 2.0; + 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); + 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); + + 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); + 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); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.75, COMMON_THRESHOLD); + + // 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_ = {}; @@ -452,6 +539,115 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) 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); + + 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); + } + } + + 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); + } + + 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, 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); + } + + 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); + + 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); + + 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); } TEST_F(SoftJointLimiterTest, check_desired_acceleration_only_cases) From f30ce41b040a3071ddb600741c0c59bc4359a791 Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Tue, 30 Apr 2024 16:04:40 +0200 Subject: [PATCH 55/71] Test uses cases for SoftJointLimiter in velocity interface --- joint_limits/src/soft_joint_limiter.cpp | 13 +- joint_limits/test/test_soft_joint_limiter.cpp | 178 +++++++++++++++--- 2 files changed, 162 insertions(+), 29 deletions(-) diff --git a/joint_limits/src/soft_joint_limiter.cpp b/joint_limits/src/soft_joint_limiter.cpp index e37b4743a5..0a668afd51 100644 --- a/joint_limits/src/soft_joint_limiter.cpp +++ b/joint_limits/src/soft_joint_limiter.cpp @@ -135,7 +135,7 @@ bool on_enforce( 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)) + 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); @@ -143,7 +143,12 @@ bool on_enforce( 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 < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) + if((position < hard_limits.min_position) || (position > hard_limits.max_position)) + { + soft_min_vel = 0.0; + soft_max_vel = 0.0; + } + else if ((position < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) { constexpr double soft_limit_reach_velocity = 1.0 * (M_PI / 180.0); soft_min_vel = std::copysign(soft_limit_reach_velocity, soft_min_vel); @@ -240,18 +245,22 @@ bool on_enforce( 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; diff --git a/joint_limits/test/test_soft_joint_limiter.cpp b/joint_limits/test/test_soft_joint_limiter.cpp index c55c956e22..8ed16007a9 100644 --- a/joint_limits/test/test_soft_joint_limiter.cpp +++ b/joint_limits/test/test_soft_joint_limiter.cpp @@ -16,6 +16,7 @@ #include "test_joint_limiter.hpp" #include +#include TEST_F(SoftJointLimiterTest, when_loading_limiter_plugin_expect_loaded) { @@ -332,9 +333,62 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) 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 + 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); @@ -347,29 +401,45 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) 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(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(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_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; @@ -540,6 +610,7 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) 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_)); @@ -562,6 +633,7 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) } } + // 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_)); @@ -577,12 +649,6 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) 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); - } - - 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, 200.0, false); test_limit_enforcing(act_pos, -0.5, 201.0, 200.0, true); @@ -592,6 +658,7 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) 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; @@ -615,6 +682,7 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) 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); @@ -631,6 +699,7 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) 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); @@ -648,6 +717,61 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) 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(SoftJointLimiterTest, check_desired_acceleration_only_cases) From c8d9eec9ab83a4cf04d2f4de5626b31c15737942 Mon Sep 17 00:00:00 2001 From: Adria Roig Date: Fri, 3 May 2024 12:12:41 +0200 Subject: [PATCH 56/71] Format changes --- .../joint_limits/joint_limits_helpers.hpp | 8 +- joint_limits/src/joint_limits_helpers.cpp | 14 +- joint_limits/src/joint_range_limiter.cpp | 2 +- joint_limits/src/soft_joint_limiter.cpp | 430 +++++++++--------- joint_limits/test/test_joint_limiter.hpp | 24 +- .../test/test_joint_range_limiter.cpp | 2 +- joint_limits/test/test_soft_joint_limiter.cpp | 14 +- 7 files changed, 251 insertions(+), 243 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index da0000a857..941e7cb077 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -17,8 +17,8 @@ #ifndef JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ #define JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ -#include "joint_limits/joint_limits.hpp" #include +#include "joint_limits/joint_limits.hpp" namespace joint_limits { @@ -38,8 +38,8 @@ std::pair compute_effort_limits( const std::optional & act_vel, double /*dt*/); std::pair compute_acceleration_limits( - const JointLimits &limits, double desired_acceleration, std::optional actual_velocity); + const JointLimits & limits, double desired_acceleration, std::optional actual_velocity); -} // namespace joint_limits +} // namespace joint_limits -#endif // JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ +#endif // JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index c9014a2a76..d6308904d3 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -14,10 +14,10 @@ /// \author Adrià Roig Moreno -#include "rclcpp/logging.hpp" #include "joint_limits/joint_limits_helpers.hpp" #include #include +#include "rclcpp/logging.hpp" namespace joint_limits { @@ -107,10 +107,12 @@ std::pair compute_effort_limits( return eff_limits; } -std::pair compute_acceleration_limits(const joint_limits::JointLimits & limits, double desired_acceleration, - std::optional actual_velocity) +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()); + 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 > 0) || @@ -119,7 +121,7 @@ std::pair compute_acceleration_limits(const joint_limits::JointL acc_or_dec_limits.first = -limits.max_deceleration; acc_or_dec_limits.second = limits.max_deceleration; } - else if(limits.has_acceleration_limits) + else if (limits.has_acceleration_limits) { acc_or_dec_limits.first = -limits.max_acceleration; acc_or_dec_limits.second = limits.max_acceleration; @@ -127,4 +129,4 @@ std::pair compute_acceleration_limits(const joint_limits::JointL return acc_or_dec_limits; } -} // namespace joint_limits +} // namespace joint_limits diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index b922fceef4..bd293d8d6f 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -18,9 +18,9 @@ #include #include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/joint_limits_helpers.hpp" #include "rclcpp/duration.hpp" #include "rcutils/logging_macros.h" -#include "joint_limits/joint_limits_helpers.hpp" namespace joint_limits { diff --git a/joint_limits/src/soft_joint_limiter.cpp b/joint_limits/src/soft_joint_limiter.cpp index 0a668afd51..f12e33364f 100644 --- a/joint_limits/src/soft_joint_limiter.cpp +++ b/joint_limits/src/soft_joint_limiter.cpp @@ -14,10 +14,10 @@ /// \author Adrià Roig Moreno -#include "joint_limits/joint_saturation_limiter.hpp" +#include #include "joint_limits/joint_limiter_struct.hpp" #include "joint_limits/joint_limits_helpers.hpp" -#include +#include "joint_limits/joint_saturation_limiter.hpp" constexpr double VALUE_CONSIDERED_ZERO = 1e-10; @@ -27,269 +27,281 @@ namespace joint_limits class SoftJointLimiter : public JointSaturationLimiter { public: - -bool on_init() -{ - const bool result = (number_of_joints_ == 1); - if (!result && has_logging_interface()) + bool on_init() { - 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) -{ - bool limits_enforced = false; - - const auto dt_seconds = dt.seconds(); - // negative or null is not allowed - if (dt_seconds <= 0.0) - { - return false; + 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; } - const auto hard_limits = joint_limits_[0]; - joint_limits::SoftJointLimits soft_joint_limits; - if(!soft_joint_limits_.empty()) + bool on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const rclcpp::Duration & dt) { - soft_joint_limits = soft_joint_limits_[0]; - } - - const std::string joint_name = joint_names_[0]; + bool limits_enforced = false; - 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()) + const auto dt_seconds = dt.seconds(); + // negative or null is not allowed + if (dt_seconds <= 0.0) { - prev_command_.acceleration = desired.acceleration; + return false; } - if (actual.has_jerk()) + + const auto hard_limits = joint_limits_[0]; + joint_limits::SoftJointLimits soft_joint_limits; + if (!soft_joint_limits_.empty()) { - prev_command_.jerk = actual.jerk; + soft_joint_limits = soft_joint_limits_[0]; } - else if (desired.has_jerk()) + + const std::string joint_name = joint_names_[0]; + + if (!prev_command_.has_data()) { - prev_command_.jerk = desired.jerk; + 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 (actual.has_data()) + + double soft_min_vel = -std::numeric_limits::infinity(); + double soft_max_vel = std::numeric_limits::infinity(); + double position = std::numeric_limits::infinity(); + + if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) { - prev_command_.joint_name = actual.joint_name; + position = prev_command_.position.value(); } - else if (desired.has_data()) + else if (actual.has_position()) { - prev_command_.joint_name = desired.joint_name; + position = actual.position.value(); } - } - - double soft_min_vel = -std::numeric_limits::infinity(); - double soft_max_vel = std::numeric_limits::infinity(); - double position = std::numeric_limits::infinity(); - if(prev_command_.has_position() && std::isfinite(prev_command_.position.value())) - { - position = prev_command_.position.value(); - } - else if(actual.has_position()) - { - position = actual.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_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) || (position > hard_limits.max_position)) + { + soft_min_vel = 0.0; + soft_max_vel = 0.0; + } + else if ( + (position < soft_joint_limits.min_position) || + (position > soft_joint_limits.max_position)) + { + constexpr 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(hard_limits.has_position_limits && has_soft_limits(soft_joint_limits) && std::isfinite(position)) + if (desired.has_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); + const auto position_limits = + compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); - soft_max_vel = std::clamp(-soft_joint_limits.k_position * (position - soft_joint_limits.max_position), - -hard_limits.max_velocity, hard_limits.max_velocity); + double pos_low = -std::numeric_limits::infinity(); + double pos_high = std::numeric_limits::infinity(); - if((position < hard_limits.min_position) || (position > hard_limits.max_position)) + if (has_soft_position_limits(soft_joint_limits)) { - soft_min_vel = 0.0; - soft_max_vel = 0.0; + pos_low = soft_joint_limits.min_position; + pos_high = soft_joint_limits.max_position; } - else if ((position < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) + + if (hard_limits.has_velocity_limits) { - constexpr 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); + 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); } - } - } - - 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(); + pos_low = std::max(pos_low, position_limits.first); + pos_high = std::min(pos_high, position_limits.second); - if(has_soft_position_limits(soft_joint_limits)) - { - pos_low = soft_joint_limits.min_position; - pos_high = soft_joint_limits.max_position; + limits_enforced = is_limited(desired.position.value(), pos_low, pos_high); + desired.position = std::clamp(desired.position.value(), pos_low, pos_high); } - if(hard_limits.has_velocity_limits) + if (desired.has_velocity()) { - 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); + const auto velocity_limits = compute_velocity_limits( + joint_name, hard_limits, actual.position, prev_command_.velocity, dt_seconds); - limits_enforced = is_limited(desired.position.value(), pos_low, pos_high); - desired.position = std::clamp(desired.position.value(), pos_low, pos_high); - } + 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); + } - if(desired.has_velocity()) - { - const auto velocity_limits = compute_velocity_limits( - joint_name, hard_limits, actual.position, prev_command_.velocity, dt_seconds); + soft_min_vel = std::max(soft_min_vel, velocity_limits.first); + soft_max_vel = std::min(soft_max_vel, velocity_limits.second); - 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); + 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); } - soft_min_vel = std::max(soft_min_vel, velocity_limits.first); - soft_max_vel = std::min(soft_max_vel, velocity_limits.second); + if (desired.has_effort()) + { + const auto effort_limits = + compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds); - 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); - } + double soft_min_eff = effort_limits.first; + double soft_max_eff = effort_limits.second; - if(desired.has_effort()) - { - const auto effort_limits = - compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds); + 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); - double soft_min_eff = effort_limits.first; - double soft_max_eff = effort_limits.second; + soft_min_eff = std::max(soft_min_eff, effort_limits.first); + soft_max_eff = std::min(soft_max_eff, effort_limits.second); + } - if(hard_limits.has_effort_limits && std::isfinite(soft_joint_limits.k_velocity) && actual.has_velocity()) + 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()) { - soft_min_eff = std::clamp(-soft_joint_limits.k_velocity * (actual.velocity.value() - soft_min_vel), - -hard_limits.max_effort, hard_limits.max_effort); + 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); + } - soft_max_eff = std::clamp(-soft_joint_limits.k_velocity * (actual.velocity.value() - soft_max_vel), - -hard_limits.max_effort, hard_limits.max_effort); + 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); + } - soft_min_eff = std::max(soft_min_eff, effort_limits.first); - soft_max_eff = std::min(soft_max_eff, effort_limits.second); + 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; } - 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); - } + prev_command_ = desired; - 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); + return limits_enforced; } - if (desired.has_jerk()) + bool has_soft_position_limits(const joint_limits::SoftJointLimits & soft_joint_limits) { - 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); + 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; } - if(desired.has_position() && !std::isfinite(desired.position.value()) && actual.has_position()) + bool has_soft_limits(const joint_limits::SoftJointLimits & soft_joint_limits) { - desired.position = actual.position; - limits_enforced = true; + 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; } - 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; -} - -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 +} // namespace joint_limits #include "pluginlib/class_list_macros.hpp" typedef joint_limits::SoftJointLimiter JointInterfacesSoftLimiter; typedef joint_limits::JointLimiterInterface JointInterfacesLimiterInterfaceBase; -PLUGINLIB_EXPORT_CLASS( - JointInterfacesSoftLimiter, JointInterfacesLimiterInterfaceBase) - - +PLUGINLIB_EXPORT_CLASS(JointInterfacesSoftLimiter, JointInterfacesLimiterInterfaceBase) diff --git a/joint_limits/test/test_joint_limiter.hpp b/joint_limits/test/test_joint_limiter.hpp index 5b909a0068..23adadbbf4 100644 --- a/joint_limits/test/test_joint_limiter.hpp +++ b/joint_limits/test/test_joint_limiter.hpp @@ -95,7 +95,7 @@ class JointLimiterTest : public ::testing::Test actual_state_.velocity.value() + desired_state_.acceleration.value() * dt; } - JointLimiterTest(const std::string &joint_limiter_type) + JointLimiterTest(const std::string & joint_limiter_type) : joint_limiter_type_(joint_limiter_type), joint_limiter_loader_( "joint_limits", @@ -103,10 +103,7 @@ class JointLimiterTest : public ::testing::Test { } - virtual ~JointLimiterTest() - { - - } + virtual ~JointLimiterTest() {} void TearDown() override { node_.reset(); } @@ -127,8 +124,7 @@ class JointLimiterTest : public ::testing::Test class JointSaturationLimiterTest : public JointLimiterTest { public: - JointSaturationLimiterTest() - : JointLimiterTest("joint_limits/JointInterfacesSaturationLimiter") + JointSaturationLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSaturationLimiter") { } }; @@ -136,14 +132,11 @@ class JointSaturationLimiterTest : public JointLimiterTest class SoftJointLimiterTest : public JointLimiterTest { public: - SoftJointLimiterTest() - : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") - { - } + SoftJointLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") {} - bool Init(const joint_limits::JointLimits & limits, - const joint_limits::SoftJointLimits &soft_limits, - const std::string & joint_name = "foo_joint") + 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(); @@ -155,8 +148,7 @@ class SoftJointLimiterTest : public JointLimiterTest 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()); + joint_names_, {limits}, {soft_limits}, nullptr, node_->get_node_logging_interface()); } }; diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 49745a3347..b78490e4da 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -14,8 +14,8 @@ /// \author Sai Kishor Kothakota -#include "test_joint_limiter.hpp" #include +#include "test_joint_limiter.hpp" TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) { diff --git a/joint_limits/test/test_soft_joint_limiter.cpp b/joint_limits/test/test_soft_joint_limiter.cpp index 8ed16007a9..34f74a8843 100644 --- a/joint_limits/test/test_soft_joint_limiter.cpp +++ b/joint_limits/test/test_soft_joint_limiter.cpp @@ -14,9 +14,9 @@ /// \author Adrià Roig Moreno -#include "test_joint_limiter.hpp" -#include #include +#include +#include "test_joint_limiter.hpp" TEST_F(SoftJointLimiterTest, when_loading_limiter_plugin_expect_loaded) { @@ -112,7 +112,8 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) 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) + // 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; @@ -256,7 +257,8 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); EXPECT_NEAR(desired_state_.position.value(), 1.75, COMMON_THRESHOLD); - // Now test when there are no position limits and soft limits, then the desired position is not saturated + // 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)); @@ -333,7 +335,7 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) EXPECT_FALSE(desired_state_.has_jerk()); }; - auto test_generic_cases = [&] (const joint_limits::SoftJointLimits &soft_joint_limits) + auto test_generic_cases = [&](const joint_limits::SoftJointLimits & soft_joint_limits) { ASSERT_TRUE(Init(limits, soft_joint_limits)); @@ -717,7 +719,7 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) 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; From 7d6ea96a1b4ee643bb7e7046db40de192fa30c74 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 May 2024 10:53:43 +0200 Subject: [PATCH 57/71] change copyright to PAL --- joint_limits/src/soft_joint_limiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits/src/soft_joint_limiter.cpp b/joint_limits/src/soft_joint_limiter.cpp index f12e33364f..f209cb44b8 100644 --- a/joint_limits/src/soft_joint_limiter.cpp +++ b/joint_limits/src/soft_joint_limiter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, PickNik Inc. +// 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. From b828b130f633928b7969f4fe3e124b3679c7ce28 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 May 2024 13:09:56 +0200 Subject: [PATCH 58/71] add pre-comitting fixes --- joint_limits/include/joint_limits/joint_limits_helpers.hpp | 2 ++ joint_limits/test/test_joint_limiter.hpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 941e7cb077..23414f4d7b 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -18,6 +18,8 @@ #define JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ #include +#include +#include #include "joint_limits/joint_limits.hpp" namespace joint_limits diff --git a/joint_limits/test/test_joint_limiter.hpp b/joint_limits/test/test_joint_limiter.hpp index 23adadbbf4..1375e052be 100644 --- a/joint_limits/test/test_joint_limiter.hpp +++ b/joint_limits/test/test_joint_limiter.hpp @@ -95,7 +95,7 @@ class JointLimiterTest : public ::testing::Test actual_state_.velocity.value() + desired_state_.acceleration.value() * dt; } - JointLimiterTest(const std::string & joint_limiter_type) + explicit JointLimiterTest(const std::string & joint_limiter_type) : joint_limiter_type_(joint_limiter_type), joint_limiter_loader_( "joint_limits", From 45328ddefe62f2d4b17242fbce5e16801f91ad34 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 May 2024 16:54:09 +0200 Subject: [PATCH 59/71] Add intermediate tests to fix the transition bug --- joint_limits/test/test_joint_range_limiter.cpp | 1 + joint_limits/test/test_soft_joint_limiter.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index b78490e4da..4db909699f 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -299,6 +299,7 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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, -0.5, -0.5, 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); diff --git a/joint_limits/test/test_soft_joint_limiter.cpp b/joint_limits/test/test_soft_joint_limiter.cpp index 34f74a8843..7530c6a693 100644 --- a/joint_limits/test/test_soft_joint_limiter.cpp +++ b/joint_limits/test/test_soft_joint_limiter.cpp @@ -487,6 +487,7 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) 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, -0.5, -0.5, 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); From b2ee4d848848a9032c6b5e300d803c1e71b9b9e3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 May 2024 22:08:57 +0200 Subject: [PATCH 60/71] Fix the undefined behavior when lower bound is greater than upper bound --- joint_limits/src/joint_limits_helpers.cpp | 24 +++++++++++++++++++ .../test/test_joint_range_limiter.cpp | 5 +++- joint_limits/test/test_soft_joint_limiter.cpp | 5 +++- 3 files changed, 32 insertions(+), 2 deletions(-) diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index d6308904d3..6d8a77b646 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -21,6 +21,19 @@ 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; } @@ -40,6 +53,7 @@ std::pair compute_position_limits( 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; } @@ -72,6 +86,15 @@ std::pair compute_velocity_limits( 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); } + RCLCPP_ERROR( + rclcpp::get_logger("joint_limiter_interface"), + "Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), vel_limits.first, + vel_limits.second); + internal::check_and_swap_limits(vel_limits); + RCLCPP_ERROR( + rclcpp::get_logger("joint_limiter_interface"), + "After swapping Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), + vel_limits.first, vel_limits.second); return vel_limits; } @@ -104,6 +127,7 @@ std::pair compute_effort_limits( eff_limits.second = 0.0; } } + internal::check_and_swap_limits(eff_limits); return eff_limits; } diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 4db909699f..4571c8ced5 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -299,7 +299,9 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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, -0.5, -0.5, 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); @@ -641,6 +643,7 @@ TEST_F(JointSaturationLimiterTest, check_all_desired_references_limiting) 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); } diff --git a/joint_limits/test/test_soft_joint_limiter.cpp b/joint_limits/test/test_soft_joint_limiter.cpp index 7530c6a693..cf55acdea8 100644 --- a/joint_limits/test/test_soft_joint_limiter.cpp +++ b/joint_limits/test/test_soft_joint_limiter.cpp @@ -487,7 +487,9 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) 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, -0.5, -0.5, 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); @@ -996,6 +998,7 @@ TEST_F(SoftJointLimiterTest, check_all_desired_references_limiting) 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); } From 8d147a11e1eafbbed1848c553a238028bd48045f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 May 2024 22:14:24 +0200 Subject: [PATCH 61/71] Add some documentation to the helpers --- .../joint_limits/joint_limits_helpers.hpp | 40 +++++++++++++++++++ joint_limits/src/joint_limits_helpers.cpp | 4 +- 2 files changed, 42 insertions(+), 2 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 23414f4d7b..32b9afbd76 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -25,20 +25,60 @@ namespace joint_limits { +/** + * @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. + */ 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. + */ 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); +/** + * @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. + */ 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. + */ std::pair compute_acceleration_limits( const JointLimits & limits, double desired_acceleration, std::optional actual_velocity); diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index 6d8a77b646..597d23598e 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -139,8 +139,8 @@ std::pair compute_acceleration_limits( -std::numeric_limits::infinity(), std::numeric_limits::infinity()); if ( limits.has_deceleration_limits && - ((desired_acceleration < 0 && actual_velocity && actual_velocity > 0) || - (desired_acceleration > 0 && actual_velocity && actual_velocity < 0))) + ((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; From a4567d497cc7f934b68691608dcee8893a2caac5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 19 Jul 2024 19:30:54 +0200 Subject: [PATCH 62/71] rename of SoftJointLimiter to JointSoftLimiter and some minor changes from manuel --- joint_limits/CMakeLists.txt | 10 +- .../include/joint_limits/joint_limits.hpp | 4 +- .../joint_limits/joint_soft_limiter.hpp | 70 ++++ joint_limits/src/joint_limits_helpers.cpp | 4 +- joint_limits/src/joint_soft_limiter.cpp | 266 +++++++++++++++ joint_limits/src/soft_joint_limiter.cpp | 307 ------------------ joint_limits/test/test_joint_limiter.hpp | 4 +- ...imiter.cpp => test_joint_soft_limiter.cpp} | 20 +- 8 files changed, 357 insertions(+), 328 deletions(-) create mode 100644 joint_limits/include/joint_limits/joint_soft_limiter.hpp create mode 100644 joint_limits/src/joint_soft_limiter.cpp delete mode 100644 joint_limits/src/soft_joint_limiter.cpp rename joint_limits/test/{test_soft_joint_limiter.cpp => test_joint_soft_limiter.cpp} (98%) diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index ac8d16580a..f88e308b21 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -57,7 +57,7 @@ ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEP add_library(joint_saturation_limiter SHARED src/joint_saturation_limiter.cpp src/joint_range_limiter.cpp - src/soft_joint_limiter.cpp + src/joint_soft_limiter.cpp ) target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) target_include_directories(joint_saturation_limiter PUBLIC @@ -117,11 +117,11 @@ if(BUILD_TESTING) rclcpp ) - ament_add_gtest(test_soft_joint_limiter test/test_soft_joint_limiter.cpp) - target_include_directories(test_soft_joint_limiter PRIVATE include) - target_link_libraries(test_soft_joint_limiter joint_limiter_interface) + ament_add_gtest(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_soft_joint_limiter + test_joint_soft_limiter pluginlib rclcpp ) 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_soft_limiter.hpp b/joint_limits/include/joint_limits/joint_soft_limiter.hpp new file mode 100644 index 0000000000..28612127b9 --- /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/joint_limiter_struct.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/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index 597d23598e..f356e92b5e 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -86,12 +86,12 @@ std::pair compute_velocity_limits( 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); } - RCLCPP_ERROR( + RCLCPP_DEBUG( rclcpp::get_logger("joint_limiter_interface"), "Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), vel_limits.first, vel_limits.second); internal::check_and_swap_limits(vel_limits); - RCLCPP_ERROR( + RCLCPP_DEBUG( rclcpp::get_logger("joint_limiter_interface"), "After swapping Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), vel_limits.first, vel_limits.second); diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp new file mode 100644 index 0000000000..b0ccbb5dd2 --- /dev/null +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -0,0 +1,266 @@ +// 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 (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) + { + position = prev_command_.position.value(); + } + else if (actual.has_position()) + { + position = actual.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) || (position > hard_limits.max_position)) + { + soft_min_vel = 0.0; + soft_max_vel = 0.0; + } + else if ( + (position < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) + { + constexpr 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, 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/src/soft_joint_limiter.cpp b/joint_limits/src/soft_joint_limiter.cpp deleted file mode 100644 index f209cb44b8..0000000000 --- a/joint_limits/src/soft_joint_limiter.cpp +++ /dev/null @@ -1,307 +0,0 @@ -// 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 -#include "joint_limits/joint_limiter_struct.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 SoftJointLimiter : public JointSaturationLimiter -{ -public: - bool 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; - } - - bool 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 (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) - { - position = prev_command_.position.value(); - } - else if (actual.has_position()) - { - position = actual.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) || (position > hard_limits.max_position)) - { - soft_min_vel = 0.0; - soft_max_vel = 0.0; - } - else if ( - (position < soft_joint_limits.min_position) || - (position > soft_joint_limits.max_position)) - { - constexpr 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, 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; - } - - 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 - -#include "pluginlib/class_list_macros.hpp" - -typedef joint_limits::SoftJointLimiter 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 index 1375e052be..33b01a52a4 100644 --- a/joint_limits/test/test_joint_limiter.hpp +++ b/joint_limits/test/test_joint_limiter.hpp @@ -129,10 +129,10 @@ class JointSaturationLimiterTest : public JointLimiterTest } }; -class SoftJointLimiterTest : public JointLimiterTest +class JointSoftLimiterTest : public JointLimiterTest { public: - SoftJointLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") {} + JointSoftLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") {} bool Init( const joint_limits::JointLimits & limits, const joint_limits::SoftJointLimits & soft_limits, diff --git a/joint_limits/test/test_soft_joint_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp similarity index 98% rename from joint_limits/test/test_soft_joint_limiter.cpp rename to joint_limits/test/test_joint_soft_limiter.cpp index cf55acdea8..d0d814dff9 100644 --- a/joint_limits/test/test_soft_joint_limiter.cpp +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -18,9 +18,9 @@ #include #include "test_joint_limiter.hpp" -TEST_F(SoftJointLimiterTest, when_loading_limiter_plugin_expect_loaded) +TEST_F(JointSoftLimiterTest, when_loading_limiter_plugin_expect_loaded) { - // Test SoftJointLimiter loading + // Test JointSoftLimiter loading ASSERT_NO_THROW( joint_limiter_ = std::unique_ptr( joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); @@ -28,7 +28,7 @@ TEST_F(SoftJointLimiterTest, when_loading_limiter_plugin_expect_loaded) } // NOTE: We accept also if there is no limit defined for a joint. -TEST_F(SoftJointLimiterTest, when_joint_not_found_expect_init_fail) +TEST_F(JointSoftLimiterTest, when_joint_not_found_expect_init_fail) { SetupNode("joint_saturation_limiter"); ASSERT_TRUE(Load()); @@ -37,7 +37,7 @@ TEST_F(SoftJointLimiterTest, when_joint_not_found_expect_init_fail) ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); } -TEST_F(SoftJointLimiterTest, when_invalid_dt_expect_enforce_fail) +TEST_F(JointSoftLimiterTest, when_invalid_dt_expect_enforce_fail) { SetupNode("joint_saturation_limiter"); ASSERT_TRUE(Load()); @@ -48,7 +48,7 @@ TEST_F(SoftJointLimiterTest, when_invalid_dt_expect_enforce_fail) ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); } -TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) +TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) { SetupNode("soft_joint_limiter"); ASSERT_TRUE(Load()); @@ -276,7 +276,7 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases) EXPECT_FALSE(desired_state_.has_jerk()); } -TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) +TEST_F(JointSoftLimiterTest, check_desired_velocity_only_cases) { SetupNode("joint_saturation_limiter"); ASSERT_TRUE(Load()); @@ -500,7 +500,7 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases) test_limit_enforcing(6.0, -1.0, 0.0, true); } -TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) +TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) { SetupNode("joint_saturation_limiter"); ASSERT_TRUE(Load()); @@ -779,7 +779,7 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases) } } -TEST_F(SoftJointLimiterTest, check_desired_acceleration_only_cases) +TEST_F(JointSoftLimiterTest, check_desired_acceleration_only_cases) { SetupNode("joint_saturation_limiter"); ASSERT_TRUE(Load()); @@ -869,7 +869,7 @@ TEST_F(SoftJointLimiterTest, check_desired_acceleration_only_cases) test_limit_enforcing(-0.4, 3.0, 0.25, true); } -TEST_F(SoftJointLimiterTest, check_desired_jerk_only_cases) +TEST_F(JointSoftLimiterTest, check_desired_jerk_only_cases) { SetupNode("joint_saturation_limiter"); ASSERT_TRUE(Load()); @@ -911,7 +911,7 @@ TEST_F(SoftJointLimiterTest, check_desired_jerk_only_cases) test_limit_enforcing(-1.5, -0.5, true); } -TEST_F(SoftJointLimiterTest, check_all_desired_references_limiting) +TEST_F(JointSoftLimiterTest, check_all_desired_references_limiting) { SetupNode("joint_saturation_limiter"); ASSERT_TRUE(Load()); From a42c972445e7205a7d140b2669f1fa9c3e4fbaf9 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 8 Jul 2024 17:19:48 +0200 Subject: [PATCH 63/71] enable moving out of position limit if velocity is in "right" direction --- .../include/joint_limits/joint_limits_helpers.hpp | 3 ++- joint_limits/src/joint_limits_helpers.cpp | 12 ++++++++---- joint_limits/src/joint_range_limiter.cpp | 3 ++- joint_limits/src/joint_soft_limiter.cpp | 3 ++- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 32b9afbd76..e9ede97d9b 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -57,7 +57,8 @@ std::pair compute_position_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 & 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. diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index f356e92b5e..fdf1c84ee3 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -59,7 +59,8 @@ std::pair compute_position_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 & 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(); @@ -70,13 +71,16 @@ std::pair compute_velocity_limits( 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) + if ( + (act_pos.value() > limits.max_position && desired_vel >= 0.0) || + (act_pos.value() < limits.min_position && desired_vel <= 0.0)) { RCLCPP_ERROR_ONCE( rclcpp::get_logger("joint_limiter_interface"), - "Joint position is out of bounds for the joint : '%s'. Joint velocity limits will be " + "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.", - joint_name.c_str()); + act_pos.value(), limits.min_position, limits.max_position, desired_vel, joint_name.c_str()); vel_limits = {0.0, 0.0}; } } diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index bd293d8d6f..3adc30d2e0 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -121,7 +121,8 @@ bool JointSaturationLimiter::on_enforce( if (desired.has_velocity()) { const auto limits = compute_velocity_limits( - joint_name, joint_limits, actual.position, prev_command_.velocity, dt_seconds); + 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); diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index b0ccbb5dd2..fdac151557 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -166,7 +166,8 @@ bool JointSoftLimiter::on_enforce( if (desired.has_velocity()) { const auto velocity_limits = compute_velocity_limits( - joint_name, hard_limits, actual.position, prev_command_.velocity, dt_seconds); + joint_name, hard_limits, desired.velocity.value(), actual.position, prev_command_.velocity, + dt_seconds); if (hard_limits.has_acceleration_limits && actual.has_velocity()) { From 46395e3640bc7be4ed21f3897608ffa8423b3e7b Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 9 Jul 2024 17:51:41 +0200 Subject: [PATCH 64/71] warn everytime we reach the limit --- joint_limits/src/joint_limits_helpers.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index fdf1c84ee3..f98752336f 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -75,8 +75,9 @@ std::pair compute_velocity_limits( (act_pos.value() > limits.max_position && desired_vel >= 0.0) || (act_pos.value() < limits.min_position && desired_vel <= 0.0)) { - RCLCPP_ERROR_ONCE( + 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.", From dc647398c32ed80471b06585e9a6979193f0e9dc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jul 2024 19:31:22 +0200 Subject: [PATCH 65/71] update the loguic to check if the actual position is within bounds and it's tolerance --- .../joint_limits/joint_limits_helpers.hpp | 4 ++ joint_limits/src/joint_limits_helpers.cpp | 46 +++++++++++++------ .../test/test_joint_range_limiter.cpp | 20 ++++++++ 3 files changed, 57 insertions(+), 13 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index e9ede97d9b..10a16729e1 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -24,6 +24,10 @@ namespace joint_limits { +namespace internal +{ +constexpr double POSITION_BOUNDS_TOLERANCE = 0.002; +} /** * @brief Checks if a value is limited by the given limits. diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index f98752336f..ed5eca07b6 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -67,22 +67,42 @@ std::pair compute_velocity_limits( 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; + 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 ( - (act_pos.value() > limits.max_position && desired_vel >= 0.0) || - (act_pos.value() < limits.min_position && desired_vel <= 0.0)) + + if (actual_pos > limits.max_position || actual_pos < limits.min_position) { - 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.", - act_pos.value(), limits.min_position, limits.max_position, desired_vel, joint_name.c_str()); - vel_limits = {0.0, 0.0}; + 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()) diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp index 4571c8ced5..083646d84f 100644 --- a/joint_limits/test/test_joint_range_limiter.cpp +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -15,6 +15,7 @@ /// \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) @@ -252,6 +253,25 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) 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; From 415f5359a474c7780eb9920de6d01c7d7442bea6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 11 Oct 2024 14:26:26 +0200 Subject: [PATCH 66/71] Remove unnecessary debug prints --- joint_limits/src/joint_limits_helpers.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index ed5eca07b6..4fee92948c 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -111,15 +111,7 @@ std::pair compute_velocity_limits( 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); } - RCLCPP_DEBUG( - rclcpp::get_logger("joint_limiter_interface"), - "Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), vel_limits.first, - vel_limits.second); internal::check_and_swap_limits(vel_limits); - RCLCPP_DEBUG( - rclcpp::get_logger("joint_limiter_interface"), - "After swapping Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), - vel_limits.first, vel_limits.second); return vel_limits; } From 2a258cff58ca439b3b2bf8580afee2ddec964f7c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 11 Oct 2024 14:27:29 +0200 Subject: [PATCH 67/71] fix the soft limiting logic and add respective tests --- joint_limits/src/joint_soft_limiter.cpp | 14 +++--- joint_limits/test/test_joint_soft_limiter.cpp | 49 +++++++++++++++---- 2 files changed, 47 insertions(+), 16 deletions(-) diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index fdac151557..a3d0d81ee9 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -96,13 +96,13 @@ bool JointSoftLimiter::on_enforce( double soft_max_vel = std::numeric_limits::infinity(); double position = std::numeric_limits::infinity(); - if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) + if (actual.has_position()) { - position = prev_command_.position.value(); + position = actual.position.value(); } - else if (actual.has_position()) + else if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) { - position = actual.position.value(); + position = prev_command_.position.value(); } if (hard_limits.has_velocity_limits) @@ -122,7 +122,9 @@ bool JointSoftLimiter::on_enforce( -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) || (position > hard_limits.max_position)) + 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; @@ -130,7 +132,7 @@ bool JointSoftLimiter::on_enforce( else if ( (position < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) { - constexpr double soft_limit_reach_velocity = 1.0 * (M_PI / 180.0); + 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); } diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp index d0d814dff9..7deb2796ea 100644 --- a/joint_limits/test/test_joint_soft_limiter.cpp +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -214,6 +214,11 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) 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); @@ -228,21 +233,40 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) 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); - soft_limits.k_position = 2.0; + // 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)); - 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 = 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; @@ -250,12 +274,17 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) 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); - 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); - desired_state_.position = 2.0; - ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); - EXPECT_NEAR(desired_state_.position.value(), 1.75, 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 From 226a855c9e506f509e9992bd4dc87db1919db931 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 11:55:18 +0100 Subject: [PATCH 68/71] Apply suggestions from code review Co-authored-by: Bence Magyar --- joint_limits/CMakeLists.txt | 4 ++-- joint_limits/src/joint_saturation_limiter.cpp | 8 +++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index f88e308b21..9729087e7f 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -108,7 +108,7 @@ if(BUILD_TESTING) rclcpp ) - ament_add_gtest(test_joint_range_limiter test/test_joint_range_limiter.cpp) + 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( @@ -117,7 +117,7 @@ if(BUILD_TESTING) rclcpp ) - ament_add_gtest(test_joint_soft_limiter test/test_joint_soft_limiter.cpp) + 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( diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 65d966ce5f..ad91793dac 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -433,10 +433,8 @@ bool JointSaturationLimiter::on_enfo // 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 - JointTrajectoryPointSaturationLimiter; -typedef joint_limits::JointLimiterInterface - JointTrajectoryPointLimiterInterfaceBase; +using int_map = std::map; +using JointTrajectoryPointSaturationLimiter = joint_limits::JointSaturationLimiter; +using JointTrajectoryPointLimiterInterfaceBase = joint_limits::JointLimiterInterface; PLUGINLIB_EXPORT_CLASS( JointTrajectoryPointSaturationLimiter, JointTrajectoryPointLimiterInterfaceBase) From 08f09ebfb4e6b32aefb8e7aadbfbd00901ed2697 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 12:22:32 +0100 Subject: [PATCH 69/71] Rename the joint_limiter_struct.hpp to data_structures.hpp --- .../{joint_limiter_struct.hpp => data_structures.hpp} | 6 +++--- joint_limits/include/joint_limits/joint_soft_limiter.hpp | 2 +- joint_limits/src/joint_range_limiter.cpp | 2 +- joint_limits/src/joint_saturation_limiter.cpp | 6 ++++-- joint_limits/test/test_joint_limiter.hpp | 2 +- 5 files changed, 10 insertions(+), 8 deletions(-) rename joint_limits/include/joint_limits/{joint_limiter_struct.hpp => data_structures.hpp} (92%) diff --git a/joint_limits/include/joint_limits/joint_limiter_struct.hpp b/joint_limits/include/joint_limits/data_structures.hpp similarity index 92% rename from joint_limits/include/joint_limits/joint_limiter_struct.hpp rename to joint_limits/include/joint_limits/data_structures.hpp index c4d7440603..b673908e67 100644 --- a/joint_limits/include/joint_limits/joint_limiter_struct.hpp +++ b/joint_limits/include/joint_limits/data_structures.hpp @@ -14,8 +14,8 @@ /// \author Sai Kishor Kothakota -#ifndef JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_ -#define JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_ +#ifndef JOINT_LIMITS__DATA_STRUCTURES_HPP_ +#define JOINT_LIMITS__DATA_STRUCTURES_HPP_ #include #include @@ -63,4 +63,4 @@ struct JointInterfacesCommandLimiterData }; } // namespace joint_limits -#endif // JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_ +#endif // JOINT_LIMITS__DATA_STRUCTURES_HPP_ diff --git a/joint_limits/include/joint_limits/joint_soft_limiter.hpp b/joint_limits/include/joint_limits/joint_soft_limiter.hpp index 28612127b9..ebc9d99934 100644 --- a/joint_limits/include/joint_limits/joint_soft_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_soft_limiter.hpp @@ -18,7 +18,7 @@ #define JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ #include -#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/data_structures.hpp" #include "joint_limits/joint_limits_helpers.hpp" #include "joint_limits/joint_saturation_limiter.hpp" diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 3adc30d2e0..1db7b3ef9a 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -17,7 +17,7 @@ #include "joint_limits/joint_saturation_limiter.hpp" #include -#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/data_structures.hpp" #include "joint_limits/joint_limits_helpers.hpp" #include "rclcpp/duration.hpp" #include "rcutils/logging_macros.h" diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index ad91793dac..a9987c4634 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -434,7 +434,9 @@ bool JointSaturationLimiter::on_enfo // 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; +using JointTrajectoryPointSaturationLimiter = + joint_limits::JointSaturationLimiter; +using JointTrajectoryPointLimiterInterfaceBase = + joint_limits::JointLimiterInterface; PLUGINLIB_EXPORT_CLASS( JointTrajectoryPointSaturationLimiter, JointTrajectoryPointLimiterInterfaceBase) diff --git a/joint_limits/test/test_joint_limiter.hpp b/joint_limits/test/test_joint_limiter.hpp index 33b01a52a4..9886598e0e 100644 --- a/joint_limits/test/test_joint_limiter.hpp +++ b/joint_limits/test/test_joint_limiter.hpp @@ -20,8 +20,8 @@ #include #include #include +#include "joint_limits/data_structures.hpp" #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" From 296f992c86d323993d02bdaca1c44cb39d60d1a1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 12:26:33 +0100 Subject: [PATCH 70/71] improve documentation of the helper methods --- .../include/joint_limits/joint_limits_helpers.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 10a16729e1..368f09002a 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -44,7 +44,7 @@ bool is_limited(double value, double min, double max); * @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. + * @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, @@ -57,7 +57,7 @@ std::pair compute_position_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. + * @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, @@ -70,7 +70,7 @@ std::pair compute_velocity_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. + * @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, @@ -82,7 +82,7 @@ std::pair compute_effort_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. + * @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); From 753133d6231192ca047ad2da5f336e52866a0c82 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 1 Jan 2025 10:14:29 +0100 Subject: [PATCH 71/71] Fix other visibility macros and pre-commit fixes --- joint_limits/include/joint_limits/joint_limiter_interface.hpp | 2 +- joint_limits/include/joint_limits/joint_saturation_limiter.hpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 584372c0d8..cbe42999ab 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -134,7 +134,7 @@ class JointLimiterInterface /** * Wrapper init method that accepts the joint names and their limits directly */ - JOINT_LIMITS_PUBLIC virtual bool init( + virtual bool init( const std::vector & joint_names, const std::vector & joint_limits, const std::vector & soft_joint_limits, diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 4c73878f55..9af574078d 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -45,8 +45,7 @@ class JointSaturationLimiter : public JointLimiterInterface