diff --git a/control_toolbox/include/control_filters/exponential_filter.hpp b/control_toolbox/include/control_filters/exponential_filter.hpp index 85d1eb13..daf917a7 100644 --- a/control_toolbox/include/control_filters/exponential_filter.hpp +++ b/control_toolbox/include/control_filters/exponential_filter.hpp @@ -18,9 +18,12 @@ #include #include #include +#include #include "filters/filter_base.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "control_toolbox/exponential_filter.hpp" #include "control_toolbox/exponential_filter_parameters.hpp" #include "control_toolbox/filters.hpp" @@ -67,7 +70,7 @@ class ExponentialFilter : public filters::FilterBase std::shared_ptr logger_; std::shared_ptr parameter_handler_; exponential_filter::Params parameters_; - T last_smoothed_value; + std::shared_ptr> expo_; }; template @@ -100,16 +103,21 @@ bool ExponentialFilter::configure() } } parameters_ = parameter_handler_->get_params(); + expo_ = std::make_shared>(parameters_.alpha); - last_smoothed_value = std::numeric_limits::quiet_NaN(); - - return true; + bool configured = expo_->configure(); + if (!configured) + { + RCLCPP_ERROR((*logger_), "ExponentialFilter: Failed to configure underlying filter instance."); + } + return configured; } -template -bool ExponentialFilter::update(const T & data_in, T & data_out) +template <> +inline bool ExponentialFilter::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!this->configured_) + if (!this->configured_ || !expo_ || !expo_->is_configured()) { throw std::runtime_error("Filter is not configured"); } @@ -118,18 +126,51 @@ bool ExponentialFilter::update(const T & data_in, T & data_out) if (parameter_handler_->is_old(parameters_)) { parameters_ = parameter_handler_->get_params(); + expo_->set_params(parameters_.alpha); + } + + // Delegate filtering to toolbox filter instance + return expo_->update(data_in, data_out); +} + +template <> +inline bool ExponentialFilter>::update( + const std::vector & data_in, std::vector & data_out) +{ + if (!this->configured_ || !expo_ || !expo_->is_configured()) + { + throw std::runtime_error("Filter is not configured"); } - if (std::isnan(last_smoothed_value)) + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) { - last_smoothed_value = data_in; + parameters_ = parameter_handler_->get_params(); + expo_->set_params(parameters_.alpha); } - data_out = last_smoothed_value = - filters::exponentialSmoothing(data_in, last_smoothed_value, parameters_.alpha); - return true; + // Delegate filtering to toolbox filter instance + return expo_->update(data_in, data_out); } +template +bool ExponentialFilter::update(const T & data_in, T & data_out) +{ + if (!this->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(); + expo_->set_params(parameters_.alpha); + } + + // Delegate filtering to toolbox filter instance + return expo_->update(data_in, data_out); +} } // namespace control_filters #endif // CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_ diff --git a/control_toolbox/include/control_toolbox/exponential_filter.hpp b/control_toolbox/include/control_toolbox/exponential_filter.hpp new file mode 100644 index 00000000..ff140cd2 --- /dev/null +++ b/control_toolbox/include/control_toolbox/exponential_filter.hpp @@ -0,0 +1,117 @@ +// 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__EXPONENTIAL_FILTER_HPP_ +#define CONTROL_TOOLBOX__EXPONENTIAL_FILTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "control_toolbox/filter_traits.hpp" + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace control_toolbox +{ +template +class ExponentialFilter +{ +public: + // Default constructor + ExponentialFilter(); + explicit ExponentialFilter(double alpha) { set_params(alpha); } + + ~ExponentialFilter(); + + bool configure(); + + bool update(const T & data_in, T & data_out); + + bool is_configured() const { return configured_; } + + void set_params(double alpha) { alpha_ = alpha; } + +private: + double alpha_; + + // Define the storage type based on T + using Traits = FilterTraits; + using StorageType = typename Traits::StorageType; + + StorageType input_value, output_value, old_value; + + bool configured_ = false; +}; + +template +ExponentialFilter::ExponentialFilter() : alpha_(0.5) +{ +} + +template +ExponentialFilter::~ExponentialFilter() +{ +} + +template +bool ExponentialFilter::configure() +{ + Traits::initialize(output_value); + Traits::initialize(input_value); + Traits::initialize(old_value); + + return configured_ = true; +} + +template +bool ExponentialFilter::update(const T & data_in, T & data_out) +{ + if (!configured_) + { + throw std::runtime_error("Filter is not configured"); + } + + // First call: initialize filter state + if (Traits::is_nan(output_value) || Traits::is_empty(output_value)) + { + if (!Traits::is_finite(data_in)) + { + return false; + } + Traits::assign(old_value, data_in); + } + else + { + Traits::validate_input(data_in, output_value, data_out); + } + + Traits::assign(input_value, data_in); + // Exponential filter update: y[n] = α * x[n] + (1-α) * y[n-1] + output_value = alpha_ * input_value + (1.0 - alpha_) * old_value; + old_value = output_value; + + Traits::assign(data_out, output_value); + Traits::add_metadata(data_out, data_in); + + return true; +} + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__EXPONENTIAL_FILTER_HPP_ diff --git a/control_toolbox/src/control_filters/exponential_filter.cpp b/control_toolbox/src/control_filters/exponential_filter.cpp index d84bc491..33965d1e 100644 --- a/control_toolbox/src/control_filters/exponential_filter.cpp +++ b/control_toolbox/src/control_filters/exponential_filter.cpp @@ -17,3 +17,10 @@ #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(control_filters::ExponentialFilter, filters::FilterBase) + +PLUGINLIB_EXPORT_CLASS( + control_filters::ExponentialFilter>, filters::FilterBase>) + +PLUGINLIB_EXPORT_CLASS( + control_filters::ExponentialFilter, + filters::FilterBase) diff --git a/control_toolbox/test/control_filters/test_exponential_filter.cpp b/control_toolbox/test/control_filters/test_exponential_filter.cpp index d98e490c..e30f913e 100644 --- a/control_toolbox/test/control_filters/test_exponential_filter.cpp +++ b/control_toolbox/test/control_filters/test_exponential_filter.cpp @@ -15,9 +15,11 @@ #include "test_filter_util.hpp" #include +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "gmock/gmock.h" #include "control_filters/exponential_filter.hpp" +#include "control_toolbox/exponential_filter.hpp" TEST_F(FilterTest, TestExponentialFilterThrowsUnconfigured) { @@ -71,6 +73,282 @@ TEST_F(FilterTest, TestExponentialFilterComputation) } } +TEST_F(FilterTest, TestExponentialWrenchFilterComputation) +{ + double alpha = 0.6; + node_->declare_parameter("alpha", rclcpp::ParameterValue(alpha)); + geometry_msgs::msg::WrenchStamped in, calculated, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.force.y = 2.0; + in.wrench.force.z = 3.0; + in.wrench.torque.x = 10.0; + in.wrench.torque.y = 20.0; + in.wrench.torque.z = 30.0; + + std::shared_ptr> filter_ = + std::make_shared>(); + + ASSERT_TRUE(filter_->configure( + "", "TestExponentialFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); + + // First filter pass, output should be equal to the input + ASSERT_TRUE(filter_->update(in, out)); + 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); + + // Initialize calculated with first output + calculated = out; + + // Second filter pass with a step in values + in.wrench.force.x = 2.0; + in.wrench.force.y = 4.0; + in.wrench.force.z = 6.0; + in.wrench.torque.x = 20.0; + in.wrench.torque.y = 40.0; + in.wrench.torque.z = 60.0; + ASSERT_TRUE(filter_->update(in, out)); + + // Manual exponential smoothing calculation for all fields + calculated.wrench.force.x = alpha * in.wrench.force.x + (1.0 - alpha) * calculated.wrench.force.x; + calculated.wrench.force.y = alpha * in.wrench.force.y + (1.0 - alpha) * calculated.wrench.force.y; + calculated.wrench.force.z = alpha * in.wrench.force.z + (1.0 - alpha) * calculated.wrench.force.z; + calculated.wrench.torque.x = + alpha * in.wrench.torque.x + (1.0 - alpha) * calculated.wrench.torque.x; + calculated.wrench.torque.y = + alpha * in.wrench.torque.y + (1.0 - alpha) * calculated.wrench.torque.y; + calculated.wrench.torque.z = + alpha * in.wrench.torque.z + (1.0 - alpha) * calculated.wrench.torque.z; + + // Third filter pass, check against manual calculation + ASSERT_TRUE(filter_->update(in, out)); + calculated.wrench.force.x = alpha * in.wrench.force.x + (1.0 - alpha) * calculated.wrench.force.x; + calculated.wrench.force.y = alpha * in.wrench.force.y + (1.0 - alpha) * calculated.wrench.force.y; + calculated.wrench.force.z = alpha * in.wrench.force.z + (1.0 - alpha) * calculated.wrench.force.z; + calculated.wrench.torque.x = + alpha * in.wrench.torque.x + (1.0 - alpha) * calculated.wrench.torque.x; + calculated.wrench.torque.y = + alpha * in.wrench.torque.y + (1.0 - alpha) * calculated.wrench.torque.y; + calculated.wrench.torque.z = + alpha * in.wrench.torque.z + (1.0 - alpha) * calculated.wrench.torque.z; + + ASSERT_NEAR(out.wrench.force.x, calculated.wrench.force.x, 1e-9); + ASSERT_NEAR(out.wrench.force.y, calculated.wrench.force.y, 1e-9); + ASSERT_NEAR(out.wrench.force.z, calculated.wrench.force.z, 1e-9); + ASSERT_NEAR(out.wrench.torque.x, calculated.wrench.torque.x, 1e-9); + ASSERT_NEAR(out.wrench.torque.y, calculated.wrench.torque.y, 1e-9); + ASSERT_NEAR(out.wrench.torque.z, calculated.wrench.torque.z, 1e-9); +} + +TEST_F(FilterTest, TestExponentialWrenchFilterDifferentAlpha) +{ + double alpha = 0.7; + node_->declare_parameter("alpha", rclcpp::ParameterValue(alpha)); + geometry_msgs::msg::WrenchStamped in, calculated, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.force.y = 2.0; + in.wrench.force.z = 3.0; + in.wrench.torque.x = 10.0; + in.wrench.torque.y = 20.0; + in.wrench.torque.z = 30.0; + + std::shared_ptr> filter_ = + std::make_shared>(); + + ASSERT_TRUE(filter_->configure( + "", "TestExponentialFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); + + // First filter pass, output should be equal to the input + ASSERT_TRUE(filter_->update(in, out)); + 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); + + // Initialize calculated with first output + calculated = out; + + // Second filter pass with a step in values + in.wrench.force.x = 2.0; + in.wrench.force.y = 4.0; + in.wrench.force.z = 6.0; + in.wrench.torque.x = 20.0; + in.wrench.torque.y = 40.0; + in.wrench.torque.z = 60.0; + ASSERT_TRUE(filter_->update(in, out)); + + // Manual exponential smoothing calculation for all fields + calculated.wrench.force.x = alpha * in.wrench.force.x + (1.0 - alpha) * calculated.wrench.force.x; + calculated.wrench.force.y = alpha * in.wrench.force.y + (1.0 - alpha) * calculated.wrench.force.y; + calculated.wrench.force.z = alpha * in.wrench.force.z + (1.0 - alpha) * calculated.wrench.force.z; + calculated.wrench.torque.x = + alpha * in.wrench.torque.x + (1.0 - alpha) * calculated.wrench.torque.x; + calculated.wrench.torque.y = + alpha * in.wrench.torque.y + (1.0 - alpha) * calculated.wrench.torque.y; + calculated.wrench.torque.z = + alpha * in.wrench.torque.z + (1.0 - alpha) * calculated.wrench.torque.z; + + // Third filter pass, check against manual calculation + ASSERT_TRUE(filter_->update(in, out)); + calculated.wrench.force.x = alpha * in.wrench.force.x + (1.0 - alpha) * calculated.wrench.force.x; + calculated.wrench.force.y = alpha * in.wrench.force.y + (1.0 - alpha) * calculated.wrench.force.y; + calculated.wrench.force.z = alpha * in.wrench.force.z + (1.0 - alpha) * calculated.wrench.force.z; + calculated.wrench.torque.x = + alpha * in.wrench.torque.x + (1.0 - alpha) * calculated.wrench.torque.x; + calculated.wrench.torque.y = + alpha * in.wrench.torque.y + (1.0 - alpha) * calculated.wrench.torque.y; + calculated.wrench.torque.z = + alpha * in.wrench.torque.z + (1.0 - alpha) * calculated.wrench.torque.z; + + ASSERT_NEAR(out.wrench.force.x, calculated.wrench.force.x, 1e-9); + ASSERT_NEAR(out.wrench.force.y, calculated.wrench.force.y, 1e-9); + ASSERT_NEAR(out.wrench.force.z, calculated.wrench.force.z, 1e-9); + ASSERT_NEAR(out.wrench.torque.x, calculated.wrench.torque.x, 1e-9); + ASSERT_NEAR(out.wrench.torque.y, calculated.wrench.torque.y, 1e-9); + ASSERT_NEAR(out.wrench.torque.z, calculated.wrench.torque.z, 1e-9); +} + +TEST_F(FilterTest, TestExponentialWrenchFilterMissingParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // should deny configuration as alpha is missing + ASSERT_FALSE(filter_->configure( + "", "TestExponentialFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); +} + +TEST_F(FilterTest, TestExponentialWrenchFilterInvalidThenFixedParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // should deny configuration as alpha is invalid + ASSERT_FALSE(filter_->configure( + "", "TestExponentialFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); + + // fix the param + node_->set_parameter(rclcpp::Parameter("alpha", 0.5)); + // should allow configuration and pass second call to unconfigured filter + ASSERT_TRUE(filter_->configure( + "", "TestExponentialFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); +} + +TEST_F(FilterTest, TestExponentialWrenchFilterThrowsUnconfigured) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + geometry_msgs::msg::WrenchStamped in, out; + ASSERT_THROW(filter_->update(in, out), std::runtime_error); +} + +TEST_F(FilterTest, TestExponentialVectorDoubleFilterComputation) +{ + // parameters should match the test yaml file + double alpha = 0.6; + + 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("alpha", rclcpp::ParameterValue(alpha)); + ASSERT_TRUE(filter_->configure( + "", "TestExponentialFilter", 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 vector by hand using exponential smoothing: y[n] = α * x[n] + (1-α) * y[n-1] + calculated[0] = alpha * in[0] + (1.0 - alpha) * out[0]; + calculated[1] = alpha * in[1] + (1.0 - alpha) * out[1]; + calculated[2] = alpha * in[2] + (1.0 - alpha) * out[2]; + // check equality with exponential filter + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_TRUE(std::equal(out.begin(), out.end(), calculated.begin())); +} + +TEST_F(FilterTest, TestExponentialFilterAlphaZero) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + node_->declare_parameter("alpha", rclcpp::ParameterValue(0.0)); + ASSERT_TRUE(filter_->configure( + "", "TestExponentialFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); + + double in = 10.0, out; + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out, 10.0); // First call initializes with input + + in = 20.0; + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out, 10.0); // Should remain at initial value with alpha=0 +} + +TEST_F(FilterTest, TestExponentialFilterAlphaOne) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + node_->declare_parameter("alpha", rclcpp::ParameterValue(1.0)); + ASSERT_TRUE(filter_->configure( + "", "TestExponentialFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); + + double in = 10.0, out; + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out, 10.0); + + in = 20.0; + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out, 20.0); // Should track input exactly with alpha=1 +} + +TEST_F(FilterTest, TestExponentialFilterParameterUpdate) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + node_->declare_parameter("alpha", rclcpp::ParameterValue(0.5)); + ASSERT_TRUE(filter_->configure( + "", "TestExponentialFilter", node_->get_node_logging_interface(), + node_->get_node_parameters_interface())); + + double in = 10.0, out; + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out, 10.0); + + // Change parameter + node_->set_parameter(rclcpp::Parameter("alpha", 0.8)); + in = 20.0; + ASSERT_TRUE(filter_->update(in, out)); + // Should use new alpha value: 0.8 * 20 + 0.2 * 10 = 18 + ASSERT_EQ(out, 18.0); +} + int main(int argc, char ** argv) { ::testing::InitGoogleMock(&argc, argv);