From 358552e64e8a7afaa3acf6a17e418f4632b57ab0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 20 Nov 2024 19:30:31 +0100 Subject: [PATCH] Move speed limiter from ros2_control repo (#212) --- CMakeLists.txt | 31 +- control_filters.xml | 9 + include/control_filters/rate_limiter.hpp | 142 +++++++ include/control_toolbox/rate_limiter.hpp | 315 ++++++++++++++++ src/control_filters/rate_limiter.cpp | 19 + .../rate_limiter_parameters.yaml | 40 ++ .../test_load_rate_limiter.cpp | 44 +++ test/rate_limiter.cpp | 348 ++++++++++++++++++ 8 files changed, 947 insertions(+), 1 deletion(-) create mode 100644 include/control_filters/rate_limiter.hpp create mode 100644 include/control_toolbox/rate_limiter.hpp create mode 100644 src/control_filters/rate_limiter.cpp create mode 100644 src/control_filters/rate_limiter_parameters.yaml create mode 100644 test/control_filters/test_load_rate_limiter.cpp create mode 100644 test/rate_limiter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8777918d..4f144e88 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,26 @@ target_link_libraries(low_pass_filter PUBLIC ) ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) +generate_parameter_library( + rate_limiter_parameters + src/control_filters/rate_limiter_parameters.yaml +) + +add_library(rate_limiter SHARED + src/control_filters/rate_limiter.cpp +) +target_compile_features(rate_limiter PUBLIC cxx_std_17) +target_include_directories(rate_limiter PUBLIC + $ + $ + $ +) +target_link_libraries(rate_limiter PUBLIC + rate_limiter_parameters + Eigen3::Eigen +) +ament_target_dependencies(rate_limiter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + # Install pluginlib xml pluginlib_export_plugin_description_file(filters control_filters.xml) @@ -92,6 +112,9 @@ if(BUILD_TESTING) ament_add_gmock(pid_tests test/pid_tests.cpp) target_link_libraries(pid_tests control_toolbox) + ament_add_gmock(rate_limiter_tests test/rate_limiter.cpp) + target_link_libraries(rate_limiter_tests control_toolbox) + ament_add_gtest(pid_parameters_tests test/pid_parameters_tests.cpp) target_link_libraries(pid_parameters_tests control_toolbox) @@ -109,6 +132,10 @@ if(BUILD_TESTING) ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter.cpp) target_link_libraries(test_load_low_pass_filter low_pass_filter low_pass_filter_parameters) ament_target_dependencies(test_load_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + ament_add_gmock(test_load_rate_limiter test/control_filters/test_load_rate_limiter.cpp) + target_link_libraries(test_load_rate_limiter rate_limiter rate_limiter_parameters) + ament_target_dependencies(test_load_rate_limiter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() # Install @@ -116,7 +143,9 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters +install(TARGETS control_toolbox + low_pass_filter low_pass_filter_parameters + rate_limiter rate_limiter_parameters EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/control_filters.xml b/control_filters.xml index a2280008..77389347 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -15,4 +15,13 @@ + + + + This is a rate limiter working with a double value. + + + diff --git a/include/control_filters/rate_limiter.hpp b/include/control_filters/rate_limiter.hpp new file mode 100644 index 00000000..2aa22072 --- /dev/null +++ b/include/control_filters/rate_limiter.hpp @@ -0,0 +1,142 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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 CONTROL_FILTERS__RATE_LIMITER_HPP_ +#define CONTROL_FILTERS__RATE_LIMITER_HPP_ + +#include +#include +#include +#include + +#include "filters/filter_base.hpp" + +#include "control_toolbox/rate_limiter.hpp" +#include "rate_limiter_parameters.hpp" + +namespace control_filters +{ + +/***************************************************/ +/*! \class RateLimiter + + \section Usage + + The RateLimiter class is meant to be instantiated as a filter in + a controller but can also be used elsewhere. + For manual instantiation, you should first call configure() + (in non-realtime) and then call update() at every update step. + +*/ +/***************************************************/ + +template +class RateLimiter : public filters::FilterBase +{ +public: + /*! + * \brief Configure the RateLimiter (access and process params). + */ + bool configure() override; + + /*! + * \brief Applies one step of the rate limiter + * + * \param data_in input to the limiter + * \param data_out limited output + * + * \returns false if filter is not configured, true otherwise + */ + bool update(const T & data_in, T & data_out) override; + +private: + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr logger_; + std::shared_ptr parameter_handler_; + rate_limiter::Params parameters_; + std::shared_ptr> limiter; + + T v1, v0; +}; + +template +bool RateLimiter::configure() +{ + clock_ = std::make_shared(RCL_SYSTEM_TIME); + logger_.reset( + new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); + + v0 = v1 = static_cast(0.0); + + // Initialize the parameters once + if (!parameter_handler_) + { + try + { + parameter_handler_ = + std::make_shared(this->params_interface_, + this->param_prefix_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "Rate limiter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "Rate limiter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + } + parameters_ = parameter_handler_->get_params(); + limiter = std::make_shared>( + parameters_.min_value, parameters_.max_value, + parameters_.min_first_derivative_neg, parameters_.max_first_derivative_pos, + parameters_.min_first_derivative_pos, parameters_.max_first_derivative_neg, + parameters_.min_second_derivative, parameters_.max_second_derivative + ); + + return true; +} + +template +bool RateLimiter::update(const T & data_in, T & data_out) +{ + if (!this->configured_ || !limiter) + { + throw std::runtime_error("Filter is not configured"); + } + + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + limiter->set_params( + parameters_.min_value, parameters_.max_value, + parameters_.min_first_derivative_neg, parameters_.max_first_derivative_pos, + parameters_.min_first_derivative_pos, parameters_.max_first_derivative_neg, + parameters_.min_second_derivative, parameters_.max_second_derivative + ); + } + T v = data_in; + limiter->limit(v, v0, v1, static_cast(parameters_.sampling_interval)); + v1 = v0; + v0 = data_in; + data_out = v; + return true; +} + +} // namespace control_filters + +#endif // CONTROL_FILTERS__RATE_LIMITER_HPP_ diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp new file mode 100644 index 00000000..bbac2fd3 --- /dev/null +++ b/include/control_toolbox/rate_limiter.hpp @@ -0,0 +1,315 @@ +// Copyright 2020 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: Enrique Fernández + */ + +#ifndef CONTROL_TOOLBOX__RATE_LIMITER_HPP_ +#define CONTROL_TOOLBOX__RATE_LIMITER_HPP_ + +#include +#include + +#include +#include + +namespace control_toolbox +{ +template +class RateLimiter +{ +public: + /** + * \brief Constructor + * + * \param [in] min_value Minimum value, e.g. [m/s], usually <= 0 + * \param [in] max_value Maximum value, e.g. [m/s], usually >= 0 + * \param [in] min_first_derivative_neg Minimum first_derivative, negative value, e.g. [m/s^2], usually <= 0 + * \param [in] max_first_derivative_pos Maximum first_derivative, positive value, e.g. [m/s^2], usually >= 0 + * \param [in] min_first_derivative_pos Asymmetric Minimum first_derivative, positive value, e.g. [m/s^2], usually <= 0 + * \param [in] max_first_derivative_neg Asymmetric Maximum first_derivative, negative value, e.g. [m/s^2], usually >= 0 + * \param [in] min_second_derivative Minimum second_derivative, e.g. [m/s^3], usually <= 0 + * \param [in] max_second_derivative Maximum second_derivative, e.g. [m/s^3], usually >= 0 + * + * \note + * If max_* values are NAN, the respective limit is deactivated + * If min_* values are NAN (unspecified), defaults to -max + * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used + */ + RateLimiter( + T min_value = std::numeric_limits::quiet_NaN(), + T max_value = std::numeric_limits::quiet_NaN(), + T min_first_derivative_neg = std::numeric_limits::quiet_NaN(), + T max_first_derivative_pos = std::numeric_limits::quiet_NaN(), + T min_first_derivative_pos = std::numeric_limits::quiet_NaN(), + T max_first_derivative_neg = std::numeric_limits::quiet_NaN(), + T min_second_derivative = std::numeric_limits::quiet_NaN(), + T max_second_derivative = std::numeric_limits::quiet_NaN()); + + /** + * \brief Limit the value and first_derivative + * \param [in, out] v value, e.g. [m/s] + * \param [in] v0 Previous value to v , e.g. [m/s] + * \param [in] v1 Previous value to v0, e.g. [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + T limit(T & v, T v0, T v1, T dt); + + /** + * \brief Limit the value + * \param [in, out] v value, e.g. [m/s] + * \return Limiting factor (1.0 if none) + */ + T limit_value(T & v); + + /** + * \brief Limit the first_derivative + * \param [in, out] v value, e.g. [m/s] + * \param [in] v0 Previous value, e.g. [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + T limit_first_derivative(T & v, T v0, T dt); + + /** + * \brief Limit the second_derivative + * \param [in, out] v value, e.g. [m/s] + * \param [in] v0 Previous value to v , e.g. [m/s] + * \param [in] v1 Previous value to v0, e.g. [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/jerk_%28physics%29#Motion_control + */ + T limit_second_derivative(T & v, T v0, T v1, T dt); + + + /** + * \brief Set the parameters + * + * \param [in] min_value Minimum value, e.g. [m/s], usually <= 0 + * \param [in] max_value Maximum value, e.g. [m/s], usually >= 0 + * \param [in] min_first_derivative_neg Minimum first_derivative, negative value, e.g. [m/s^2], usually <= 0 + * \param [in] max_first_derivative_pos Maximum first_derivative, positive value, e.g. [m/s^2], usually >= 0 + * \param [in] min_first_derivative_pos Asymmetric Minimum first_derivative, positive value, e.g. [m/s^2], usually <= 0 + * \param [in] max_first_derivative_neg Asymmetric Maximum first_derivative, negative value, e.g. [m/s^2], usually >= 0 + * \param [in] min_second_derivative Minimum second_derivative, e.g. [m/s^3], usually <= 0 + * \param [in] max_second_derivative Maximum second_derivative, e.g. [m/s^3], usually >= 0 + * + * \note + * If max_* values are NAN, the respective limit is deactivated + * If min_* values are NAN (unspecified), defaults to -max + * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used + */ + void set_params( + T min_value = std::numeric_limits::quiet_NaN(), + T max_value = std::numeric_limits::quiet_NaN(), + T min_first_derivative_neg = std::numeric_limits::quiet_NaN(), + T max_first_derivative_pos = std::numeric_limits::quiet_NaN(), + T min_first_derivative_pos = std::numeric_limits::quiet_NaN(), + T max_first_derivative_neg = std::numeric_limits::quiet_NaN(), + T min_second_derivative = std::numeric_limits::quiet_NaN(), + T max_second_derivative = std::numeric_limits::quiet_NaN()); + +private: + // Enable/Disable value/first_derivative/second_derivative limits: + bool has_value_limits_ = true; + bool has_first_derivative_limits_ = true; + bool has_second_derivative_limits_ = true; + + // value limits: + T min_value_ = std::numeric_limits::quiet_NaN(); + T max_value_ = std::numeric_limits::quiet_NaN(); + + // first_derivative limits: + T min_first_derivative_neg_ = std::numeric_limits::quiet_NaN(); + T max_first_derivative_pos_ = std::numeric_limits::quiet_NaN(); + T min_first_derivative_pos_ = std::numeric_limits::quiet_NaN(); + T max_first_derivative_neg_ = std::numeric_limits::quiet_NaN(); + + // second_derivative limits: + T min_second_derivative_ = std::numeric_limits::quiet_NaN(); + T max_second_derivative_ = std::numeric_limits::quiet_NaN(); +}; + +template +RateLimiter::RateLimiter( + T min_value, T max_value, + T min_first_derivative_neg, T max_first_derivative_pos, + T min_first_derivative_pos, T max_first_derivative_neg, + T min_second_derivative, T max_second_derivative) +{ + set_params( + min_value, max_value, + min_first_derivative_neg, max_first_derivative_pos, + min_first_derivative_pos, max_first_derivative_neg, + min_second_derivative, max_second_derivative + ); +}; + +// Check if limits are valid, max must be specified, min defaults to -max if unspecified +template +void RateLimiter::set_params( + T min_value, T max_value, + T min_first_derivative_neg, T max_first_derivative_pos, + T min_first_derivative_pos, T max_first_derivative_neg, + T min_second_derivative, T max_second_derivative) + { + min_value_ = min_value; + max_value_ = max_value; + min_first_derivative_neg_ = min_first_derivative_neg; + max_first_derivative_pos_ = max_first_derivative_pos; + min_first_derivative_pos_ = min_first_derivative_pos; + max_first_derivative_neg_ = max_first_derivative_neg; + min_second_derivative_ = min_second_derivative; + max_second_derivative_ = max_second_derivative; + + if (std::isnan(max_value_)) + { + has_value_limits_ = false; + } + if (std::isnan(min_value_)) + { + min_value_ = -max_value_; + } + if (has_value_limits_ && min_value_ > max_value_) + { + throw std::invalid_argument("Invalid value limits"); + } + + if (std::isnan(max_first_derivative_pos_)) + { + has_first_derivative_limits_ = false; + } + if (std::isnan(min_first_derivative_neg_)) + { + min_first_derivative_neg_ = -max_first_derivative_pos_; + } + if (has_first_derivative_limits_ && min_first_derivative_neg_ > max_first_derivative_pos_) + { + throw std::invalid_argument("Invalid first derivative limits"); + } + if (has_first_derivative_limits_) + { + if (std::isnan(max_first_derivative_neg_)) + { + max_first_derivative_neg_ = max_first_derivative_pos_; + } + if (std::isnan(min_first_derivative_pos_)) + { + min_first_derivative_pos_ = min_first_derivative_neg_; + } + if (has_first_derivative_limits_ && min_first_derivative_pos_ > max_first_derivative_neg_) + { + throw std::invalid_argument("Invalid first derivative limits"); + } + } + + if (std::isnan(max_second_derivative_)) + { + has_second_derivative_limits_ = false; + } + if (std::isnan(min_second_derivative_)) + { + min_second_derivative_ = -max_second_derivative_; + } + if (has_second_derivative_limits_ && min_second_derivative_ > max_second_derivative_) + { + throw std::invalid_argument("Invalid second derivative limits"); + } +} + +template +T RateLimiter::limit(T & v, T v0, T v1, T dt) +{ + const T tmp = v; + + limit_second_derivative(v, v0, v1, dt); + limit_first_derivative(v, v0, dt); + limit_value(v); + + return tmp != static_cast(0.0) ? v / tmp : static_cast(1.0); +} + +template +T RateLimiter::limit_value(T & v) +{ + const T tmp = v; + + if (has_value_limits_) + { + v = std::clamp(v, min_value_, max_value_); + } + + return tmp != static_cast(0.0) ? v / tmp : static_cast(1.0); +} + +template +T RateLimiter::limit_first_derivative(T & v, T v0, T dt) +{ + const T tmp = v; + + if (has_first_derivative_limits_) + { + T dv_max, dv_min = 0; + if (v0 > static_cast(0.0)) + { + dv_max = max_first_derivative_pos_ * dt; + dv_min = min_first_derivative_pos_ * dt; + } + else if (v0 < static_cast(0.0)) + { + dv_min = min_first_derivative_neg_ * dt; + dv_max = max_first_derivative_neg_ * dt; + } + else + { + dv_min = min_first_derivative_neg_ * dt; + dv_max = max_first_derivative_pos_ * dt; + } + const T dv = std::clamp(v - v0, dv_min, dv_max); + + v = v0 + dv; + } + + return tmp != static_cast(0.0) ? v / tmp : static_cast(1.0); +} + +template +T RateLimiter::limit_second_derivative(T & v, T v0, T v1, T dt) +{ + const T tmp = v; + + if (has_second_derivative_limits_) + { + const T dv = v - v0; + const T dv0 = v0 - v1; + + const T dt2 = static_cast(2.0) * dt * dt; + + const T da_min = min_second_derivative_ * dt2; + const T da_max = max_second_derivative_ * dt2; + + const T da = std::clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != static_cast(0.0) ? v / tmp : static_cast(1.0); +} + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__RATE_LIMITER_HPP_ diff --git a/src/control_filters/rate_limiter.cpp b/src/control_filters/rate_limiter.cpp new file mode 100644 index 00000000..55cb1a31 --- /dev/null +++ b/src/control_filters/rate_limiter.cpp @@ -0,0 +1,19 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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. + +#include "control_filters/rate_limiter.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::RateLimiter, filters::FilterBase) diff --git a/src/control_filters/rate_limiter_parameters.yaml b/src/control_filters/rate_limiter_parameters.yaml new file mode 100644 index 00000000..ec9027b7 --- /dev/null +++ b/src/control_filters/rate_limiter_parameters.yaml @@ -0,0 +1,40 @@ +rate_limiter: + sampling_interval: { + type: double, + description: "Sampling interval in seconds", + validation: { + gt<>: [0.0] + }, + } + max_value: { + type: double, + default_value: .NAN, + } + min_value: { + type: double, + default_value: .NAN, + } + max_first_derivative_pos: { + type: double, + default_value: .NAN, + } + min_first_derivative_pos: { + type: double, + default_value: .NAN, + } + max_first_derivative_neg: { + type: double, + default_value: .NAN, + } + min_first_derivative_neg: { + type: double, + default_value: .NAN, + } + max_second_derivative: { + type: double, + default_value: .NAN, + } + min_second_derivative: { + type: double, + default_value: .NAN, + } diff --git a/test/control_filters/test_load_rate_limiter.cpp b/test/control_filters/test_load_rate_limiter.cpp new file mode 100644 index 00000000..649e1983 --- /dev/null +++ b/test/control_filters/test_load_rate_limiter.cpp @@ -0,0 +1,44 @@ +// 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. + +#include +#include +#include + +#include "rclcpp/utilities.hpp" +#include "pluginlib/class_loader.hpp" + +#include "control_filters/rate_limiter.hpp" + +TEST(TestLoadRateLimtier, load_rate_limiter_double) +{ + rclcpp::init(0, nullptr); + + pluginlib::ClassLoader> filter_loader("filters", + "filters::FilterBase"); + std::shared_ptr> filter; + auto available_classes = filter_loader.getDeclaredClasses(); + std::stringstream sstr; + sstr << "available filters:" << std::endl; + for (const auto& available_class : available_classes) + { + sstr << " " << available_class << std::endl; + } + + std::string filter_type = "control_filters/RateLimiterDouble"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + + rclcpp::shutdown(); +} diff --git a/test/rate_limiter.cpp b/test/rate_limiter.cpp new file mode 100644 index 00000000..f333e07a --- /dev/null +++ b/test/rate_limiter.cpp @@ -0,0 +1,348 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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. + +#include +#include + +#include "control_toolbox/rate_limiter.hpp" + + +TEST(RateLimiterTest, testWrongParams) +{ + // different value limits + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, std::numeric_limits::quiet_NaN(), + -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0)); + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0)); + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + std::numeric_limits::quiet_NaN(), 1.0, + -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0)); + EXPECT_THROW(control_toolbox::RateLimiter limiter( + 1.0, -1.0, + -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0), + std::invalid_argument); + + // different limits for first derivative + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, + -1.0, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0)); + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0)); + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, + std::numeric_limits::quiet_NaN(), 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0)); + EXPECT_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, + 1.0, -1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0), + std::invalid_argument); + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, + -1.0, 1.0, + -10.0, std::numeric_limits::quiet_NaN(), + -1.0, 1.0)); + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, + -1.0, 1.0, + std::numeric_limits::quiet_NaN(), 10.0, + -1.0, 1.0)); + EXPECT_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, + 1.0, -1.0, + 10.0, -10.0, + -1.0, 1.0), + std::invalid_argument); + + // different limits for second derivative + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, std::numeric_limits::quiet_NaN())); + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN())); + EXPECT_NO_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), 1.0)); + EXPECT_THROW(control_toolbox::RateLimiter limiter( + -1.0, 1.0, + -1.0, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + 1.0, -1.0), + std::invalid_argument); +} + +TEST(RateLimiterTest, testNoLimits) +{ + control_toolbox::RateLimiter limiter; + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the first_derivative is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the value is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); +} + +TEST(RateLimiterTest, testValueLimits) +{ + control_toolbox::RateLimiter limiter( + -0.5, 1.0, + -0.5, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -0.5, 5.0); + + { + double v = 10.0; + double limiting_factor = limiter.limit_value(v); + // check if the robot speed is now 1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 1.0); + EXPECT_DOUBLE_EQ(limiting_factor, 0.1); + v = -10.0; + limiting_factor = limiter.limit_value(v); + // check if the robot speed is now -0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + } + + { + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // first_derivative is now limiting, not value + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // first_derivative is now limiting, not value + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + } +} + +TEST(RateLimiterTest, testValueNoLimits) +{ + { + control_toolbox::RateLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -0.5, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -0.5, 5.0 + ); + double v = 10.0; + double limiting_factor = limiter.limit_value(v); + // check if the value is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit_value(v); + // check if the value is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } + + { + control_toolbox::RateLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -0.5, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -0.5, 5.0 + ); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // first_derivative is now limiting, not value + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // first_derivative is now limiting, not value + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + } + + { + control_toolbox::RateLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -0.5, 5.0 + ); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // second_derivative is now limiting, not value + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5/10.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // second_derivative is now limiting, not value + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + } + + { + control_toolbox::RateLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the value is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the value is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } +} + +TEST(RateLimiterTest, testFirstDerivativeLimits) +{ + control_toolbox::RateLimiter limiter( -0.5, 1.0, + -0.5, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -0.5, 5.0 + ); + + { + double v = 10.0; + double limiting_factor = limiter.limit_first_derivative(v, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + + v = -10.0; + limiting_factor = limiter.limit_first_derivative(v, 0.0, 0.5); + // check if the robot speed is now -0.25 m.s-1, which is -0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + } + + { + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.25 m.s-1, which is -0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + } +} + +TEST(RateLimiterTest, testFirstDerivativeLimitsAsymmetric) +{ + control_toolbox::RateLimiter limiter( -0.5, 1.0, + -0.5, 1.0, + -5.0, 10.0, + -0.5, 5.0 + ); + + { + double v = 10.0; + double v0 = 5.0; + double limiting_factor = limiter.limit_first_derivative(v, v0, 0.5); + // check if the robot speed is now 5.5 m.s-1, which is 5.0 + 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 5.5); + EXPECT_DOUBLE_EQ(limiting_factor, 5.5/10.0); + + v = -10.0; + limiting_factor = limiter.limit_first_derivative(v, v0, 0.5); + // check if the robot speed is now 2.5 m.s-1, which is 5.0 - 5.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, -2.5/10.0); + } + + { + double v = 10.0; + double v0 = -5.0; + double limiting_factor = limiter.limit_first_derivative(v, v0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is -5 + 10.m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.0); + EXPECT_DOUBLE_EQ(limiting_factor, 0.0); // div by 0 + + v = -10.0; + limiting_factor = limiter.limit_first_derivative(v, v0, 0.5); + // check if the robot speed is now -0.25 m.s-1, which is -5 - 0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -5.25); + EXPECT_DOUBLE_EQ(limiting_factor, 5.25/10.0); + } +} + +TEST(RateLimiterTest, testSecondDerivativeLimits) +{ + control_toolbox::RateLimiter limiter( + -0.5, 1.0, + -0.5, 1.0, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + -1.0, 1.0 + ); + + { + double v = 10.0; + double limiting_factor = limiter.limit_second_derivative(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5m.s-1 = 1.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + v = -10.0; + limiting_factor = limiter.limit_second_derivative(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5m.s-1 = -1.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + } + { + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5m.s-1 = 1.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // first_derivative is limiting, not second_derivative + // check if the robot speed is now -0.25 m.s-1, which is -0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + } +}