From cf041a3c686c6db7f4e80d1e692c8790f8e18eea Mon Sep 17 00:00:00 2001 From: marqrazz Date: Tue, 22 Apr 2025 09:32:46 -0600 Subject: [PATCH 1/5] Add std::vector option to the low pass filter --- .../control_filters/low_pass_filter.hpp | 20 ++++++++++++ .../control_toolbox/low_pass_filter.hpp | 31 +++++++++++++++++++ 2 files changed, 51 insertions(+) diff --git a/control_toolbox/include/control_filters/low_pass_filter.hpp b/control_toolbox/include/control_filters/low_pass_filter.hpp index 1614cb8e..00d559d7 100644 --- a/control_toolbox/include/control_filters/low_pass_filter.hpp +++ b/control_toolbox/include/control_filters/low_pass_filter.hpp @@ -153,6 +153,26 @@ inline bool LowPassFilter::update( return lpf_->update(data_in, data_out); } +template <> +inline bool LowPassFilter>::update( + const std::vector & data_in, std::vector & data_out) +{ + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) + { + throw std::runtime_error("Filter is not configured"); + } + + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + lpf_->set_params( + parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); + } + + return lpf_->update(data_in, data_out); +} + template bool LowPassFilter::update(const T & data_in, T & data_out) { diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index 3085c7fa..08fbcbe8 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -124,6 +124,8 @@ class LowPassFilter double b1_; /** feedforward coefficient. */ /** internal data storage (double). */ double filtered_value, filtered_old_value, old_value; + /** internal data storage (std::vector). */ + std::vector vector_filtered_value, vector_filtered_old_value, vector_old_value; /** internal data storage (wrench). */ Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; bool configured_ = false; @@ -186,6 +188,35 @@ inline bool LowPassFilter::update( return true; } +template <> +inline bool LowPassFilter>::update( + const std::vector & data_in, std::vector & data_out) +{ + if (!configured_) + { + throw std::runtime_error("Filter is not configured"); + } + // If this is the first call to update initialize the filter at the current state + // so that we dont apply an impulse to the data. + // This also sets the size of the member variables to match the input data. + if (vector_filtered_value.empty()) + { + vector_filtered_value = data_in; + vector_filtered_old_value = data_in; + vector_old_value = data_in; + } + + // Filter each value in the vector + for (std::size_t i = 0; i < data_in.size(); i++) + { + data_out[i] = b1_ * vector_old_value[i] + a1_ * vector_filtered_old_value[i]; + vector_filtered_old_value[i] = data_out[i]; + vector_old_value[i] = data_in[i]; + } + + return true; +} + template bool LowPassFilter::update(const T & data_in, T & data_out) { From be00266164e3ccb6163ab1bd424ecb257298de4b Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 23 Apr 2025 08:18:16 -0600 Subject: [PATCH 2/5] update to be non-zero initialized and add tests --- control_toolbox/control_filters.xml | 7 ++ .../control_toolbox/low_pass_filter.hpp | 52 ++++++++--- .../src/control_filters/low_pass_filter.cpp | 3 + .../test_load_low_pass_filter.cpp | 22 +++++ .../control_filters/test_low_pass_filter.cpp | 86 +++++++++++++------ 5 files changed, 130 insertions(+), 40 deletions(-) diff --git a/control_toolbox/control_filters.xml b/control_toolbox/control_filters.xml index 5791dee2..ebe88b43 100644 --- a/control_toolbox/control_filters.xml +++ b/control_toolbox/control_filters.xml @@ -18,6 +18,13 @@ This is a low pass filter working with a double value. + + + This is a low pass filter working with a std::vector double value. + + diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index 08fbcbe8..c7c42b13 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -122,10 +122,8 @@ class LowPassFilter // Filter parameters double a1_; /** feedbackward coefficient. */ double b1_; /** feedforward coefficient. */ - /** internal data storage (double). */ - double filtered_value, filtered_old_value, old_value; - /** internal data storage (std::vector). */ - std::vector vector_filtered_value, vector_filtered_old_value, vector_old_value; + /** internal data storage of template type. */ + T filtered_value, filtered_old_value, old_value; /** internal data storage (wrench). */ Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; bool configured_ = false; @@ -145,11 +143,11 @@ template bool LowPassFilter::configure() { // Initialize storage Vectors - filtered_value = filtered_old_value = old_value = 0; + filtered_value = filtered_old_value = old_value = std::numeric_limits::quiet_NaN(); // TODO(destogl): make the size parameterizable and more intelligent is using complex types for (Eigen::Index i = 0; i < 6; ++i) { - msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; + msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = std::numeric_limits::quiet_NaN(); } return configured_ = true; @@ -163,6 +161,19 @@ inline bool LowPassFilter::update( { throw std::runtime_error("Filter is not configured"); } + // If this is the first call to update initialize the filter at the current state + // so that we dont apply an impulse to the data. + if (msg_filtered.hasNaN()) + { + msg_filtered[0] = data_in.wrench.force.x; + msg_filtered[1] = data_in.wrench.force.y; + msg_filtered[2] = data_in.wrench.force.z; + msg_filtered[3] = data_in.wrench.torque.x; + msg_filtered[4] = data_in.wrench.torque.y; + msg_filtered[5] = data_in.wrench.torque.z; + msg_filtered_old = msg_filtered; + msg_old = msg_filtered; + } // IIR Filter msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old; @@ -199,19 +210,26 @@ inline bool LowPassFilter>::update( // If this is the first call to update initialize the filter at the current state // so that we dont apply an impulse to the data. // This also sets the size of the member variables to match the input data. - if (vector_filtered_value.empty()) + if (filtered_value.empty()) + { + filtered_value = data_in; + filtered_old_value = data_in; + old_value = data_in; + } + else { - vector_filtered_value = data_in; - vector_filtered_old_value = data_in; - vector_old_value = data_in; + assert( + data_in.size() == filtered_value.size() && + "Internal data and the data_in doesn't hold the same size"); + assert(data_out.size() == data_in.size() && "data_in and data_out doesn't hold same size"); } // Filter each value in the vector for (std::size_t i = 0; i < data_in.size(); i++) { - data_out[i] = b1_ * vector_old_value[i] + a1_ * vector_filtered_old_value[i]; - vector_filtered_old_value[i] = data_out[i]; - vector_old_value[i] = data_in[i]; + data_out[i] = b1_ * old_value[i] + a1_ * filtered_old_value[i]; + filtered_old_value[i] = data_out[i]; + old_value[i] = data_in[i]; } return true; @@ -224,6 +242,14 @@ bool LowPassFilter::update(const T & data_in, T & data_out) { throw std::runtime_error("Filter is not configured"); } + // If this is the first call to update initialize the filter at the current state + // so that we dont apply an impulse to the data. + if (filtered_value == std::numeric_limits::quiet_NaN()) + { + filtered_value = data_in; + filtered_old_value = data_in; + old_value = data_in; + } // Filter data_out = b1_ * old_value + a1_ * filtered_old_value; diff --git a/control_toolbox/src/control_filters/low_pass_filter.cpp b/control_toolbox/src/control_filters/low_pass_filter.cpp index d5063722..e8606bf1 100644 --- a/control_toolbox/src/control_filters/low_pass_filter.cpp +++ b/control_toolbox/src/control_filters/low_pass_filter.cpp @@ -18,6 +18,9 @@ PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + control_filters::LowPassFilter>, filters::FilterBase>) + PLUGINLIB_EXPORT_CLASS( control_filters::LowPassFilter, filters::FilterBase) diff --git a/control_toolbox/test/control_filters/test_load_low_pass_filter.cpp b/control_toolbox/test/control_filters/test_load_low_pass_filter.cpp index bc656431..8e965f87 100644 --- a/control_toolbox/test/control_filters/test_load_low_pass_filter.cpp +++ b/control_toolbox/test/control_filters/test_load_low_pass_filter.cpp @@ -43,6 +43,28 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_double) rclcpp::shutdown(); } +TEST(TestLoadLowPassFilter, load_low_pass_filter_vector_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/LowPassFilterVectorDouble"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + + rclcpp::shutdown(); +} + TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) { rclcpp::init(0, nullptr); diff --git a/control_toolbox/test/control_filters/test_low_pass_filter.cpp b/control_toolbox/test/control_filters/test_low_pass_filter.cpp index 286a331d..7103b84f 100644 --- a/control_toolbox/test/control_filters/test_low_pass_filter.cpp +++ b/control_toolbox/test/control_filters/test_low_pass_filter.cpp @@ -110,23 +110,28 @@ TEST_F(FilterTest, TestLowPassWrenchFilterComputation) "", "TestLowPassFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); - // first filter pass, output should be zero as no old wrench was stored + // first filter pass, output should be equal to the input ASSERT_TRUE(filter_->update(in, out)); - ASSERT_EQ(out.wrench.force.x, 0.0); - ASSERT_EQ(out.wrench.force.y, 0.0); - ASSERT_EQ(out.wrench.force.z, 0.0); - ASSERT_EQ(out.wrench.torque.x, 0.0); - ASSERT_EQ(out.wrench.torque.y, 0.0); - ASSERT_EQ(out.wrench.torque.z, 0.0); - - // second filter pass with same values: + ASSERT_EQ(out.wrench.force.x, in.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, in.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, in.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, in.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, in.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, in.wrench.torque.z); + + // second filter pass with a step in values (otherwise there is nothing to filter): + in.wrench.force.x = 2.0; + in.wrench.torque.x = 20.0; + ASSERT_TRUE(filter_->update(in, out)); + + // update once again and check results // calculate wrench by hand - calculated.wrench.force.x = b1 * in.wrench.force.x; - calculated.wrench.force.y = b1 * in.wrench.force.y; - calculated.wrench.force.z = b1 * in.wrench.force.z; - calculated.wrench.torque.x = b1 * in.wrench.torque.x; - calculated.wrench.torque.y = b1 * in.wrench.torque.y; - calculated.wrench.torque.z = b1 * in.wrench.torque.z; + calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * out.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * out.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * out.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * out.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * out.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * out.wrench.torque.z; // check equality with low-pass-filter ASSERT_TRUE(filter_->update(in, out)); ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); @@ -135,23 +140,50 @@ TEST_F(FilterTest, TestLowPassWrenchFilterComputation) ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); +} + +TEST_F(FilterTest, TestLowPassVectorDoubleFilterComputation) +{ + // parameters should match the test yaml file + double sampling_freq = 1000.0; + double damping_freq = 20.5; + double damping_intensity = 1.25; + + double a1, b1; + a1 = exp( + -1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0))); + b1 = 1.0 - a1; + + std::vector in{1.0, 2.0, 3.0}; + std::vector calculated, out; + calculated.resize(in.size()); + out.resize(in.size()); + + std::shared_ptr>> filter_ = + std::make_shared>>(); + + node_->declare_parameter("sampling_frequency", rclcpp::ParameterValue(sampling_freq)); + node_->declare_parameter("damping_frequency", rclcpp::ParameterValue(damping_freq)); + node_->declare_parameter("damping_intensity", rclcpp::ParameterValue(damping_intensity)); + ASSERT_TRUE(filter_->configure( + "", "TestLowPassFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); + + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_TRUE(std::equal(in.begin(), in.end(), out.begin())); + + // second filter pass with a step in values (otherwise there is nothing to filter): + in = {2.0, 4.0, 6.0}; + ASSERT_TRUE(filter_->update(in, out)); // update once again and check results // calculate wrench by hand - calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * calculated.wrench.force.x; - calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * calculated.wrench.force.y; - calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * calculated.wrench.force.z; - calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * calculated.wrench.torque.x; - calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * calculated.wrench.torque.y; - calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * calculated.wrench.torque.z; + calculated[0] = b1 * in[0] + a1 * out[0]; + calculated[1] = b1 * in[1] + a1 * out[1]; + calculated[2] = b1 * in[2] + a1 * out[2]; // check equality with low-pass-filter ASSERT_TRUE(filter_->update(in, out)); - ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); - ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); - ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); - ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); - ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); - ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); + ASSERT_TRUE(std::equal(out.begin(), out.end(), calculated.begin())); } int main(int argc, char ** argv) From 3403da049e0ad2adb6e553b429ef686cb8616762 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 23 Apr 2025 09:56:04 -0600 Subject: [PATCH 3/5] pre-commit --- control_toolbox/include/control_filters/low_pass_filter.hpp | 1 + control_toolbox/include/control_toolbox/low_pass_filter.hpp | 1 + 2 files changed, 2 insertions(+) diff --git a/control_toolbox/include/control_filters/low_pass_filter.hpp b/control_toolbox/include/control_filters/low_pass_filter.hpp index 00d559d7..f0d79309 100644 --- a/control_toolbox/include/control_filters/low_pass_filter.hpp +++ b/control_toolbox/include/control_filters/low_pass_filter.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "filters/filter_base.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index c7c42b13..615edc30 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include From 4a0dbaa1fbcb3a83f42aef2aff6591d501012f74 Mon Sep 17 00:00:00 2001 From: Marq Rasmussen Date: Wed, 23 Apr 2025 13:06:45 -0600 Subject: [PATCH 4/5] Update control_toolbox/include/control_toolbox/low_pass_filter.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- control_toolbox/include/control_toolbox/low_pass_filter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index 615edc30..e3a97835 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -245,7 +245,7 @@ bool LowPassFilter::update(const T & data_in, T & data_out) } // If this is the first call to update initialize the filter at the current state // so that we dont apply an impulse to the data. - if (filtered_value == std::numeric_limits::quiet_NaN()) + if (std::isnan(filtered_value)) { filtered_value = data_in; filtered_old_value = data_in; From 0ba8382e79733ce45dbe6e0e8e54c90e8661a60b Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 23 Apr 2025 14:28:28 -0600 Subject: [PATCH 5/5] protect against incomming NaN data --- control_toolbox/include/control_toolbox/low_pass_filter.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index e3a97835..50ad55e7 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -230,7 +230,7 @@ inline bool LowPassFilter>::update( { data_out[i] = b1_ * old_value[i] + a1_ * filtered_old_value[i]; filtered_old_value[i] = data_out[i]; - old_value[i] = data_in[i]; + if (std::isfinite(data_in[i])) old_value[i] = data_in[i]; } return true; @@ -255,7 +255,7 @@ bool LowPassFilter::update(const T & data_in, T & data_out) // Filter data_out = b1_ * old_value + a1_ * filtered_old_value; filtered_old_value = data_out; - old_value = data_in; + if (std::isfinite(data_in)) old_value = data_in; return true; }