diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index e4a11782..830dbb0d 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -21,11 +21,12 @@ #include #include -#include "low_pass_filter_parameters.hpp" #include "filters/filter_base.hpp" - #include "geometry_msgs/msg/wrench_stamped.hpp" +#include "control_toolbox/low_pass_filter.hpp" +#include "low_pass_filter_parameters.hpp" + namespace control_filters { @@ -79,14 +80,6 @@ template class LowPassFilter : public filters::FilterBase { public: - // Default constructor - LowPassFilter(); - - /*! - * \brief Destructor of LowPassFilter class. - */ - ~LowPassFilter() override; - /*! * \brief Configure the LowPassFilter (access and process params). */ @@ -102,44 +95,14 @@ class LowPassFilter : public filters::FilterBase */ bool update(const T & data_in, T & data_out) override; -protected: - /*! - * \brief Internal computation of the feedforward and feedbackward coefficients - * according to the LowPassFilter parameters. - */ - void compute_internal_params() - { - a1_ = exp( - -1.0 / parameters_.sampling_frequency * (2.0 * M_PI * parameters_.damping_frequency) / - (pow(10.0, parameters_.damping_intensity / -10.0))); - b1_ = 1.0 - a1_; - }; - private: rclcpp::Clock::SharedPtr clock_; std::shared_ptr logger_; std::shared_ptr parameter_handler_; low_pass_filter::Params parameters_; - - // Filter parameters - /** internal data storage (double). */ - double filtered_value, filtered_old_value, old_value; - /** internal data storage (wrench). */ - Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; - double a1_; /**< feedbackward coefficient. */ - double b1_; /**< feedforward coefficient. */ + std::shared_ptr> lpf_; }; -template -LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) -{ -} - -template -LowPassFilter::~LowPassFilter() -{ -} - template bool LowPassFilter::configure() { @@ -168,24 +131,19 @@ bool LowPassFilter::configure() } } parameters_ = parameter_handler_->get_params(); - compute_internal_params(); + lpf_ = std::make_shared>( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); - // Initialize storage Vectors - filtered_value = filtered_old_value = old_value = 0; - // TODO(destogl): make the size parameterizable and more intelligent is using complex types - for (size_t i = 0; i < 6; ++i) - { - msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; - } - - return true; + return lpf_->configure(); } template <> inline bool LowPassFilter::update( const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!this->configured_) + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { if (logger_) RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); @@ -196,39 +154,22 @@ inline bool LowPassFilter::update( if (parameter_handler_->is_old(parameters_)) { parameters_ = parameter_handler_->get_params(); - compute_internal_params(); + lpf_->set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); } - // IIR Filter - msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old; - msg_filtered_old = msg_filtered; - - // TODO(destogl): use wrenchMsgToEigen - msg_old[0] = data_in.wrench.force.x; - msg_old[1] = data_in.wrench.force.y; - msg_old[2] = data_in.wrench.force.z; - msg_old[3] = data_in.wrench.torque.x; - msg_old[4] = data_in.wrench.torque.y; - msg_old[5] = data_in.wrench.torque.z; - - data_out.wrench.force.x = msg_filtered[0]; - data_out.wrench.force.y = msg_filtered[1]; - data_out.wrench.force.z = msg_filtered[2]; - data_out.wrench.torque.x = msg_filtered[3]; - data_out.wrench.torque.y = msg_filtered[4]; - data_out.wrench.torque.z = msg_filtered[5]; - - // copy the header - data_out.header = data_in.header; - return true; + return lpf_->update(data_in, data_out); } template bool LowPassFilter::update(const T & data_in, T & data_out) { - if (!this->configured_) + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + if (logger_) + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } @@ -236,15 +177,13 @@ bool LowPassFilter::update(const T & data_in, T & data_out) if (parameter_handler_->is_old(parameters_)) { parameters_ = parameter_handler_->get_params(); - compute_internal_params(); + lpf_->set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); } - // Filter - data_out = b1_ * old_value + a1_ * filtered_old_value; - filtered_old_value = data_out; - old_value = data_in; - - return true; + return lpf_->update(data_in, data_out); } } // namespace control_filters diff --git a/include/control_toolbox/low_pass_filter.hpp b/include/control_toolbox/low_pass_filter.hpp new file mode 100644 index 00000000..df214d09 --- /dev/null +++ b/include/control_toolbox/low_pass_filter.hpp @@ -0,0 +1,224 @@ +// 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 CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ +#define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace control_toolbox +{ + +/***************************************************/ +/*! \class LowPassFilter + \brief A Low-pass filter class. + + This class implements a low-pass filter for + various data types based on an Infinite Impulse Response Filter. + For vector elements, the filtering is applied separately on + each element of the vector. + + In particular, this class implements a simplified version of + an IIR filter equation : + + \f$y(n) = b x(n-1) + a y(n-1)\f$ + + where:
+
    +
  • \f$ x(n)\f$ is the input signal +
  • \f$ y(n)\f$ is the output signal (filtered) +
  • \f$ b \f$ is the feedforward filter coefficient +
  • \f$ a \f$ is the feedback filter coefficient +
+ + and the Low-Pass coefficient equation: +
+
    +
  • \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$ +
  • \f$ b = 1 - a \f$ +
+ + where:
+
    +
  • \f$ sf \f$ is the sampling frequency +
  • \f$ df \f$ is the damping frequency +
  • \f$ di \f$ is the damping intensity (amplitude) +
+ + \section Usage + + For manual instantiation, you should first call configure() + (in non-realtime) and then call update() at every update step. + +*/ +/***************************************************/ + +template +class LowPassFilter +{ +public: + // Default constructor + LowPassFilter(); + + LowPassFilter(double sampling_frequency, double damping_frequency, double damping_intensity){ + set_params(sampling_frequency, damping_frequency, damping_intensity); + } + + /*! + * \brief Destructor of LowPassFilter class. + */ + ~LowPassFilter(); + + /*! + * \brief Configure the LowPassFilter (access and process params). + */ + bool configure(); + + /*! + * \brief Applies one iteration of the IIR filter. + * + * \param data_in input to the filter + * \param data_out filtered output + * + * \returns false if filter is not configured, true otherwise + */ + bool update(const T & data_in, T & data_out); + + bool set_params( + const double sampling_frequency, + const double damping_frequency, + const double damping_intensity) + { + // TODO(roncapat): parameters validation + this->sampling_frequency = sampling_frequency; + this->damping_frequency = damping_frequency; + this->damping_intensity = damping_intensity; + compute_internal_params(); + return true; + } + + bool is_configured() const + { + return configured_; + } + +protected: + /*! + * \brief Internal computation of the feedforward and feedbackward coefficients + * according to the LowPassFilter parameters. + */ + void compute_internal_params() + { + a1_ = exp( + -1.0 / sampling_frequency * (2.0 * M_PI * damping_frequency) / + (pow(10.0, damping_intensity / -10.0))); + b1_ = 1.0 - a1_; + }; + +private: + // Filter parameters + /** internal data storage (double). */ + double filtered_value, filtered_old_value, old_value; + /** internal data storage (wrench). */ + Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + double sampling_frequency, damping_frequency, damping_intensity; + double a1_; /** feedbackward coefficient. */ + double b1_; /** feedforward coefficient. */ + bool configured_ = false; +}; + +template +LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) +{ +} + +template +LowPassFilter::~LowPassFilter() +{ +} + +template +bool LowPassFilter::configure() +{ + compute_internal_params(); + + // Initialize storage Vectors + filtered_value = filtered_old_value = old_value = 0; + // TODO(destogl): make the size parameterizable and more intelligent is using complex types + for (size_t i = 0; i < 6; ++i) + { + msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; + } + + return configured_ = true; +} + +template <> +inline bool LowPassFilter::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) +{ + if (!configured_) + { + return false; + } + + // IIR Filter + msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old; + msg_filtered_old = msg_filtered; + + // TODO(destogl): use wrenchMsgToEigen + msg_old[0] = data_in.wrench.force.x; + msg_old[1] = data_in.wrench.force.y; + msg_old[2] = data_in.wrench.force.z; + msg_old[3] = data_in.wrench.torque.x; + msg_old[4] = data_in.wrench.torque.y; + msg_old[5] = data_in.wrench.torque.z; + + data_out.wrench.force.x = msg_filtered[0]; + data_out.wrench.force.y = msg_filtered[1]; + data_out.wrench.force.z = msg_filtered[2]; + data_out.wrench.torque.x = msg_filtered[3]; + data_out.wrench.torque.y = msg_filtered[4]; + data_out.wrench.torque.z = msg_filtered[5]; + + // copy the header + data_out.header = data_in.header; + return true; +} + +template +bool LowPassFilter::update(const T & data_in, T & data_out) +{ + if (!configured_) + { + return false; + } + + // Filter + data_out = b1_ * old_value + a1_ * filtered_old_value; + filtered_old_value = data_out; + old_value = data_in; + + return true; +} + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp index f3c00fa8..91eb3568 100644 --- a/test/control_filters/test_load_low_pass_filter.cpp +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -15,11 +15,12 @@ #include #include #include -#include "control_filters/low_pass_filter.hpp" + #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/utilities.hpp" -#include +#include "pluginlib/class_loader.hpp" +#include "control_filters/low_pass_filter.hpp" TEST(TestLoadLowPassFilter, load_low_pass_filter_double) { @@ -38,7 +39,7 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_double) std::string filter_type = "control_filters/LowPassFilterDouble"; ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); - ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); rclcpp::shutdown(); } @@ -60,7 +61,7 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) std::string filter_type = "control_filters/LowPassFilterWrench"; ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); - ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); rclcpp::shutdown(); }