From 91620454cd392ef367b681ebb3b109f56c5b63ef Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 6 Apr 2024 23:53:31 +0200 Subject: [PATCH 01/41] 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 efaf529724..8611ef53d7 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -30,9 +30,8 @@ namespace joint_limits { -using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint; -template +template class JointLimiterInterface { public: diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 733178d695..1946cc7a4f 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -36,8 +36,8 @@ namespace joint_limits * limit. For example, if a joint is close to its position limit, velocity and acceleration will be * reduced accordingly. */ -template -class JointSaturationLimiter : public JointLimiterInterface +template +class JointSaturationLimiter : public JointLimiterInterface { public: /** \brief Constructor */ @@ -89,14 +89,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 5020cab27b..d2401d62ba 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -27,7 +27,7 @@ constexpr double VALUE_CONSIDERED_ZERO = 1e-10; namespace joint_limits { template <> -bool JointSaturationLimiter::on_enforce( +bool JointSaturationLimiter::on_enforce( trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) { @@ -428,7 +428,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; @@ -473,6 +474,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 950b611109..61a38ab340 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -34,7 +34,8 @@ const double COMMON_THRESHOLD = 0.0011; EXPECT_NEAR(tested_traj_point.velocities[idx], expected_vel, COMMON_THRESHOLD); \ EXPECT_NEAR(tested_traj_point.accelerations[idx], expected_acc, COMMON_THRESHOLD); -using JointLimiter = joint_limits::JointLimiterInterface; +using JointLimiter = joint_limits::JointLimiterInterface< + joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint>; class JointSaturationLimiterTest : public ::testing::Test { @@ -84,7 +85,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 fb0e7d4de502d0900c42ad74d14011f4f3b78a5b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 09:48:39 +0200 Subject: [PATCH 02/41] 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 8611ef53d7..a77d402179 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -69,55 +69,58 @@ class JointLimiterInterface // TODO(destogl): get limits from URDF // Initialize and get joint limits from parameter server - for (size_t i = 0; i < number_of_joints_; ++i) + if (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 f8c6fcd57b5308565a25f6e4cced64aad5727055 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 09:52:12 +0200 Subject: [PATCH 03/41] 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 a77d402179..191684649a 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -69,7 +69,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) { @@ -236,6 +236,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 940ff312fa8fa98d7a894d0416c7703bc808c83d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 10:29:24 +0200 Subject: [PATCH 04/41] 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 191684649a..b77906717a 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -132,6 +132,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. @@ -239,18 +257,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 0ed50bb6b62e8b55222d2fb09feacdc189362450 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 10:49:01 +0200 Subject: [PATCH 05/41] 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 1946cc7a4f..4a80616772 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -49,7 +49,7 @@ class JointSaturationLimiter : public JointLimiterInterface Date: Sun, 7 Apr 2024 11:39:07 +0200 Subject: [PATCH 06/41] 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 b77906717a..8baf5afa61 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -147,7 +147,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 f2914860d1635bfdf3c0c55d6c126568571ac725 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 16:57:10 +0200 Subject: [PATCH 07/41] 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 44128f633e..37afc88778 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -56,6 +56,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) @@ -103,6 +116,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 e87cad2237ee97a43e6454cc9828ce2af1e9086b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 20:14:31 +0200 Subject: [PATCH 08/41] 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 3a2ddc5d9d4c0e03e841a79f25fd4cc6eb99eed0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 20:20:30 +0200 Subject: [PATCH 09/41] 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 39d904ab6446f1d1d34e7b8299aa06612a68c2a0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 22:17:30 +0200 Subject: [PATCH 10/41] 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 a1ca0508601e579a371fac40631fa1febd51fabd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 7 Apr 2024 22:28:56 +0200 Subject: [PATCH 11/41] 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 a7e98522887b464a74eece03423b2465daf543c9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 23:08:03 +0200 Subject: [PATCH 12/41] 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 8baf5afa61..aeab1e4ae4 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -206,19 +206,6 @@ class JointLimiterInterface return on_enforce(current_joint_states, desired_joint_states, dt); } - /** \brief Enforce joint limits to desired joint state for single physical quantity. - * - * Generic enforce method that calls implementation-specific `on_enforce` method. - * - * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. - * \returns true if limits are enforced, otherwise false. - */ - JOINT_LIMITS_PUBLIC virtual bool enforce(std::vector & desired_joint_states) - { - joint_limits_ = *(updated_limits_.readFromRT()); - return on_enforce(desired_joint_states); - } - protected: /** \brief Method is realized by an implementation. * @@ -249,18 +236,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 4a80616772..bb1b2070bf 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -73,17 +73,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 d2401d62ba..433ff58133 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -427,55 +427,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 e78c4b8994..72cecfcbf8 100644 --- a/joint_limits/test/test_joint_saturation_limiter.cpp +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -517,48 +517,6 @@ TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_exp } } -TEST_F(JointSaturationLimiterTest, when_there_are_effort_limits_expect_them_to_be_applyed) -{ - SetupNode("joint_saturation_limiter"); - Load(); - - if (joint_limiter_) - { - Init(); - Configure(); - - // value not over limit - desired_joint_states_.effort[0] = 15.0; - ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); - - // value over limit - desired_joint_states_.effort[0] = 21.0; - ASSERT_TRUE(joint_limiter_->enforce(desired_joint_states_.effort)); - ASSERT_EQ(desired_joint_states_.effort[0], 20.0); - } -} - -TEST_F(JointSaturationLimiterTest, when_there_are_no_effort_limits_expect_them_not_applyed) -{ - SetupNode("joint_saturation_limiter"); - Load(); - - if (joint_limiter_) - { - Init("foo_joint_no_effort"); - Configure(); - - // value not over limit - desired_joint_states_.effort[0] = 15.0; - ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); - - // value over limit - desired_joint_states_.effort[0] = 21.0; - ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort)); - ASSERT_EQ(desired_joint_states_.effort[0], 21.0); - } -} - int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From dadc9581888b4a082edbdedbe6e4cc05eeceaa59 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 23:13:07 +0200 Subject: [PATCH 13/41] 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 aeab1e4ae4..62f2a6e4d2 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -238,11 +238,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 05636859afe79975ad75dba4b53a66ba568cd2ff Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 23:41:54 +0200 Subject: [PATCH 14/41] 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 37afc88778..5f8e380b47 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -45,6 +45,7 @@ target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDIN add_library(joint_saturation_limiter SHARED src/joint_saturation_limiter.cpp + src/joint_range_limiter.cpp ) target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) target_include_directories(joint_saturation_limiter PUBLIC @@ -56,19 +57,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) @@ -116,7 +104,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 bb1b2070bf..9dbebb997b 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -49,8 +49,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 cc67fbaaf80baa34e32cfbaba837ce285c5d15ff Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 23:52:56 +0200 Subject: [PATCH 15/41] 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 433ff58133..31a0d91a54 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -436,9 +436,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 61a38ab340..ebb11ecf27 100644 --- a/joint_limits/test/test_joint_saturation_limiter.hpp +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -83,7 +83,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/41] 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 5f8e380b47..1317420ecb 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -94,6 +94,15 @@ if(BUILD_TESTING) rclcpp ) + ament_add_gtest(test_joint_range_limiter test/test_joint_range_limiter.cpp) + target_include_directories(test_joint_range_limiter PRIVATE include) + target_link_libraries(test_joint_range_limiter joint_limiter_interface) + ament_target_dependencies( + test_joint_saturation_limiter + pluginlib + rclcpp + ) + endif() install( diff --git a/joint_limits/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 4278adb0089e0731ef4a0084de300b1950adb4b0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 21:44:30 +0200 Subject: [PATCH 17/41] 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 afade53a8615f9348600eef08c5075f82c4ba797 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 22:12:54 +0200 Subject: [PATCH 18/41] 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 255be09978b35090ee4bd5c0040db2836b21c249 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 22:55:41 +0200 Subject: [PATCH 19/41] 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 a47ba74334a0f8acdc6daebe49bb9f320b9f4df4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 22:56:10 +0200 Subject: [PATCH 20/41] 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 1a78beb09da878940dbfe4958cc235a3d45f0c3a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 23:10:49 +0200 Subject: [PATCH 21/41] 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 0f48afafe8911eb95c6bd70be168c29a6d173d85 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 23:48:13 +0200 Subject: [PATCH 22/41] 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 71df02345f123f809fbd7bc1bccbb86acc691cf4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 23:48:42 +0200 Subject: [PATCH 23/41] 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 358e3bd9eda062409f8e8395c83c5b5ca64e3b55 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 09:22:21 +0200 Subject: [PATCH 24/41] 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 f26bd8d499a8f431d4429c571dc927874a40e1e0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 10:12:17 +0200 Subject: [PATCH 25/41] 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 7bc19c059216cafba3bbcb92c5840e434f6fe5d4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 10:12:52 +0200 Subject: [PATCH 26/41] 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 8a5267a9668102d9d6f569fe73889bd9170f972c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 10:17:27 +0200 Subject: [PATCH 27/41] 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 332d403ed6164679fb3b439a985e53744b86ef82 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 10:41:47 +0200 Subject: [PATCH 28/41] 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 df6bef029c22720ad6e4993013bc3d862c243fcb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 12:08:49 +0200 Subject: [PATCH 29/41] 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 faba81d18621b7c6751a3923878d7041c840d6b2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 12:09:47 +0200 Subject: [PATCH 30/41] 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 97e18b83969ef281ceb1cc54ee21f6a674002518 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 12:23:46 +0200 Subject: [PATCH 31/41] 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 0b476f37648f7988613fa1852cc0b95b10eee897 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 12:24:31 +0200 Subject: [PATCH 32/41] 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 e31f6a45e0b8950f4ef7e1d7ca932503b5d2bb72 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 13:15:17 +0200 Subject: [PATCH 33/41] 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 732dae6ead27020d9a746b03f0a9ba796063dd94 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 13:15:39 +0200 Subject: [PATCH 34/41] 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 d120117d353a8ed85c5100e56b457e61d8ed9bc0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 18:05:39 +0200 Subject: [PATCH 35/41] 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 f4741d1944307e121284f544778ffd66e724d7c1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 18:05:59 +0200 Subject: [PATCH 36/41] 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 bc97448fc0e746be07c897c80e909869aafe280b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 18:16:07 +0200 Subject: [PATCH 37/41] 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 12973f68037db37807b946443f843deb862e5daf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 23:35:43 +0200 Subject: [PATCH 38/41] 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 a973d427cd37efa81fc03bbd17f1358fa35a9c73 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 23:36:19 +0200 Subject: [PATCH 39/41] 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 6d8d66eab88b3233ad367016d3c48e25844b1d37 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 19 Apr 2024 00:23:30 +0200 Subject: [PATCH 40/41] 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 415ea203e1c7d1c129b69b4369f1bbe4faa894ef Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 19 Apr 2024 00:23:46 +0200 Subject: [PATCH 41/41] 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);