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_filters/low_pass_filter.hpp b/control_toolbox/include/control_filters/low_pass_filter.hpp index 1614cb8e..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" @@ -153,6 +154,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..50ad55e7 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 @@ -122,8 +123,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 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; @@ -143,11 +144,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; @@ -161,6 +162,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; @@ -186,6 +200,42 @@ 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 (filtered_value.empty()) + { + filtered_value = data_in; + filtered_old_value = data_in; + old_value = data_in; + } + else + { + 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_ * old_value[i] + a1_ * filtered_old_value[i]; + filtered_old_value[i] = data_out[i]; + if (std::isfinite(data_in[i])) old_value[i] = data_in[i]; + } + + return true; +} + template bool LowPassFilter::update(const T & data_in, T & data_out) { @@ -193,11 +243,19 @@ 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 (std::isnan(filtered_value)) + { + filtered_value = data_in; + filtered_old_value = data_in; + old_value = data_in; + } // 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; } 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)