From d21cfccabe02e5ff72f7623f42fa4c7d6edb749b Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Sun, 27 Apr 2025 02:13:08 +0200 Subject: [PATCH 01/12] Make member variables of LowPassFilter class generic Signed-off-by: Pedro Nogueira --- .../control_toolbox/low_pass_filter.hpp | 112 ++++++++++-------- 1 file changed, 63 insertions(+), 49 deletions(-) diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index 82d40b15..192fcb6d 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "geometry_msgs/msg/wrench_stamped.hpp" @@ -123,10 +124,17 @@ class LowPassFilter // Filter parameters double a1_; /** feedbackward coefficient. */ double b1_; /** feedforward coefficient. */ - /** 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; + + // Define the storage type based on T + using StorageType = typename std::conditional< + std::is_same::value, Eigen::Matrix, + T>::type; + + // Member variables + StorageType filtered_value_; + StorageType filtered_old_value_; + StorageType old_value_; + bool configured_ = false; }; @@ -143,12 +151,18 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - // Initialize storage Vectors - 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) + // Initialize storage Vectors, depending on the type of T + if constexpr (std::is_same::value) + { + // TODO(destogl): make the size parameterizable and more intelligent is using complex types + for (Eigen::Index i = 0; i < 6; ++i) + { + filtered_value_[i] = filtered_old_value_[i] = old_value_[i] = std::numeric_limits::quiet_NaN(); + } + } + else { - msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = std::numeric_limits::quiet_NaN(); + filtered_value_ = filtered_old_value_ = old_value_ = std::numeric_limits::quiet_NaN(); } return configured_ = true; @@ -164,36 +178,36 @@ 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. - if (msg_filtered.hasNaN()) + if (filtered_value_.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; + filtered_value_[0] = data_in.wrench.force.x; + filtered_value_[1] = data_in.wrench.force.y; + filtered_value_[2] = data_in.wrench.force.z; + filtered_value_[3] = data_in.wrench.torque.x; + filtered_value_[4] = data_in.wrench.torque.y; + filtered_value_[5] = data_in.wrench.torque.z; + filtered_old_value_ = filtered_value_; + old_value_ = filtered_value_; } // IIR Filter - msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old; - msg_filtered_old = msg_filtered; + filtered_value_ = b1_ * old_value_ + a1_ * filtered_old_value_; + filtered_old_value_ = filtered_value_; // 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]; + filtered_old_value_[0] = data_in.wrench.force.x; + filtered_old_value_[1] = data_in.wrench.force.y; + filtered_old_value_[2] = data_in.wrench.force.z; + filtered_old_value_[3] = data_in.wrench.torque.x; + filtered_old_value_[4] = data_in.wrench.torque.y; + filtered_old_value_[5] = data_in.wrench.torque.z; + + data_out.wrench.force.x = filtered_value_[0]; + data_out.wrench.force.y = filtered_value_[1]; + data_out.wrench.force.z = filtered_value_[2]; + data_out.wrench.torque.x = filtered_value_[3]; + data_out.wrench.torque.y = filtered_value_[4]; + data_out.wrench.torque.z = filtered_value_[5]; // copy the header data_out.header = data_in.header; @@ -211,20 +225,20 @@ 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 (filtered_value.empty()) + if (filtered_value_.empty()) { if (std::any_of(data_in.begin(), data_in.end(), [](double val) { return !std::isfinite(val); })) { return false; } - filtered_value = data_in; - filtered_old_value = data_in; - old_value = data_in; + filtered_value_ = data_in; + filtered_old_value_ = data_in; + old_value_ = data_in; } else { assert( - data_in.size() == filtered_value.size() && + 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"); } @@ -232,11 +246,11 @@ inline bool LowPassFilter>::update( // 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]; + 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]; + old_value_[i] = data_in[i]; } } @@ -252,19 +266,19 @@ 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 (std::isnan(filtered_value)) + if (std::isnan(filtered_value_)) { - filtered_value = data_in; - filtered_old_value = data_in; - old_value = data_in; + 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; + data_out = b1_ * old_value_ + a1_ * filtered_old_value_; + filtered_old_value_ = data_out; if (std::isfinite(data_in)) { - old_value = data_in; + old_value_ = data_in; } return true; @@ -272,4 +286,4 @@ bool LowPassFilter::update(const T & data_in, T & data_out) } // namespace control_toolbox -#endif // CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ +#endif // CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ \ No newline at end of file From 1612ade63d5cbd1f04f444a58284b76024dd1393 Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Sun, 27 Apr 2025 10:33:06 +0200 Subject: [PATCH 02/12] Run pre-commit and format code Signed-off-by: Pedro Nogueira --- control_toolbox/include/control_toolbox/low_pass_filter.hpp | 5 +++-- 1 file changed, 3 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 192fcb6d..82d34f9c 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -157,7 +157,8 @@ bool LowPassFilter::configure() // TODO(destogl): make the size parameterizable and more intelligent is using complex types for (Eigen::Index i = 0; i < 6; ++i) { - filtered_value_[i] = filtered_old_value_[i] = old_value_[i] = std::numeric_limits::quiet_NaN(); + filtered_value_[i] = filtered_old_value_[i] = old_value_[i] = + std::numeric_limits::quiet_NaN(); } } else @@ -286,4 +287,4 @@ bool LowPassFilter::update(const T & data_in, T & data_out) } // namespace control_toolbox -#endif // CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ \ No newline at end of file +#endif // CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ From 72a80bae8b7ba5056cfe77dfc6560fef527cc2de Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Sun, 27 Apr 2025 16:22:26 +0200 Subject: [PATCH 03/12] Add filter traits structure Signed-off-by: Pedro Nogueira --- .../include/control_toolbox/filter_traits.hpp | 54 +++++++++++++++++++ .../control_toolbox/low_pass_filter.hpp | 29 +++------- 2 files changed, 62 insertions(+), 21 deletions(-) create mode 100644 control_toolbox/include/control_toolbox/filter_traits.hpp diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp new file mode 100644 index 00000000..d8a2e648 --- /dev/null +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -0,0 +1,54 @@ +// 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__FILTER_TRAITS_HPP_ +#define CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ + +#define EIGEN_INITIALIZE_MATRICES_BY_NAN + +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace control_toolbox +{ + +// Default: storage type is just T itself +template +struct FilterTraits +{ + using StorageType = T; + + static void initialize(StorageType & storage) + { + storage = T{std::numeric_limits::quiet_NaN()}; + } +}; + +// Specialization: for WrenchStamped, use struct Vec6, wrapper for Eigen::Matrix +template <> +struct FilterTraits +{ + using StorageType = Eigen::Matrix; + + static void initialize(StorageType & storage) + { + // Evocation of the default constructor through EIGEN_INITIALIZE_MATRICES_BY_NAN + storage = StorageType(); + } +}; +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index 82d34f9c..c6a20b88 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -25,6 +25,8 @@ #include #include +#include "control_toolbox/filter_traits.hpp" + #include "geometry_msgs/msg/wrench_stamped.hpp" namespace control_toolbox @@ -126,14 +128,10 @@ class LowPassFilter double b1_; /** feedforward coefficient. */ // Define the storage type based on T - using StorageType = typename std::conditional< - std::is_same::value, Eigen::Matrix, - T>::type; + using Traits = FilterTraits; + using StorageType = typename Traits::StorageType; - // Member variables - StorageType filtered_value_; - StorageType filtered_old_value_; - StorageType old_value_; + StorageType filtered_value_, filtered_old_value_, old_value_; bool configured_ = false; }; @@ -151,20 +149,9 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - // Initialize storage Vectors, depending on the type of T - if constexpr (std::is_same::value) - { - // TODO(destogl): make the size parameterizable and more intelligent is using complex types - for (Eigen::Index i = 0; i < 6; ++i) - { - filtered_value_[i] = filtered_old_value_[i] = old_value_[i] = - std::numeric_limits::quiet_NaN(); - } - } - else - { - filtered_value_ = filtered_old_value_ = old_value_ = std::numeric_limits::quiet_NaN(); - } + Traits::initialize(filtered_value_); + Traits::initialize(filtered_old_value_); + Traits::initialize(old_value_); return configured_ = true; } From 68a550d18ebfd5300fc34d84977d809ad0ac50cd Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Mon, 28 Apr 2025 17:54:19 +0200 Subject: [PATCH 04/12] Make LowPassFilter::update() function generic Signed-off-by: Pedro Nogueira --- .../include/control_toolbox/filter_traits.hpp | 152 ++++++++++++++++++ .../control_toolbox/low_pass_filter.hpp | 111 +++---------- 2 files changed, 175 insertions(+), 88 deletions(-) diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp index d8a2e648..2ad8cb3b 100644 --- a/control_toolbox/include/control_toolbox/filter_traits.hpp +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -18,13 +18,59 @@ #define EIGEN_INITIALIZE_MATRICES_BY_NAN #include +#include +#include #include +#include #include "geometry_msgs/msg/wrench_stamped.hpp" namespace control_toolbox { +// Forward declaration of FilterTraits +template +struct FilterTraits; + +// Wrapper around std::vector to be used as the std::vector StorageType specialization +// This is a workaround for the fact that std::vector's operator* and operator+ cannot be overloaded. +struct FilterVector +{ + std::vector data; + + FilterVector() = default; + + explicit FilterVector(const std::vector & vec) : data(vec) {} + + explicit FilterVector(size_t size, double initial_value = 0.0) : data(size, initial_value) {} + + FilterVector operator*(double scalar) const + { + FilterVector result = *this; + for (auto & val : result.data) + { + val *= scalar; + } + return result; + } + + FilterVector operator+(const FilterVector & other) const + { + assert(data.size() == other.data.size() && "Vectors must be of the same size for addition"); + FilterVector result = *this; + for (size_t i = 0; i < data.size(); ++i) + { + result.data[i] += other.data[i]; + } + return result; + } + + size_t size() const { return data.size(); } +}; + +// Enable scalar * FilterVector +inline FilterVector operator*(double scalar, const FilterVector & vec) { return vec * scalar; } + // Default: storage type is just T itself template struct FilterTraits @@ -35,6 +81,18 @@ struct FilterTraits { storage = T{std::numeric_limits::quiet_NaN()}; } + + static bool is_nan(const StorageType & storage) { return std::isnan(storage); } + + static bool is_infinite(const StorageType & storage) { return !std::isfinite(storage); } + + static bool is_empty(const StorageType & storage) + { + (void)storage; + return false; + } + + static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } }; // Specialization: for WrenchStamped, use struct Vec6, wrapper for Eigen::Matrix @@ -42,13 +100,107 @@ template <> struct FilterTraits { using StorageType = Eigen::Matrix; + using DataType = geometry_msgs::msg::WrenchStamped; static void initialize(StorageType & storage) { // Evocation of the default constructor through EIGEN_INITIALIZE_MATRICES_BY_NAN storage = StorageType(); } + + static bool is_nan(const StorageType & storage) { return storage.hasNaN(); } + + static bool is_infinite(const DataType & data) + { + return !std::isfinite(data.wrench.force.x) || !std::isfinite(data.wrench.force.y) || + !std::isfinite(data.wrench.force.z) || !std::isfinite(data.wrench.torque.x) || + !std::isfinite(data.wrench.torque.y) || !std::isfinite(data.wrench.torque.z); + } + + static bool is_valid(const StorageType & storage) + { + return std::all_of( + storage.begin(), storage.end(), [](double val) { return std::isfinite(val); }); + } + + static bool is_empty(const StorageType & storage) + { + (void)storage; + return false; + } + + static void assign(DataType & data_in, const StorageType & storage) + { + data_in.wrench.force.x = storage[0]; + data_in.wrench.force.y = storage[1]; + data_in.wrench.force.z = storage[2]; + data_in.wrench.torque.x = storage[3]; + data_in.wrench.torque.y = storage[4]; + data_in.wrench.torque.z = storage[5]; + } + + static void assign(StorageType & storage, const DataType & data_in) + { + storage[0] = data_in.wrench.force.x; + storage[1] = data_in.wrench.force.y; + storage[2] = data_in.wrench.force.z; + storage[3] = data_in.wrench.torque.x; + storage[4] = data_in.wrench.torque.y; + storage[5] = data_in.wrench.torque.z; + } + + static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } +}; + +// Specialization: for std::vector +template <> +struct FilterTraits> +{ + using StorageType = FilterVector; + using DataType = std::vector; + + static void initialize(StorageType & storage) + { + storage.data = std::vector{std::numeric_limits::quiet_NaN()}; + } + + static bool is_infinite(const StorageType & storage) + { + return std::all_of( + storage.data.begin(), storage.data.end(), [](double val) { return std::isfinite(val); }); + } + + static bool is_infinite(const DataType & storage) + { + return std::all_of( + storage.begin(), storage.end(), [](double val) { return std::isfinite(val); }); + } + + static bool is_empty(const StorageType & storage) { return storage.data.empty(); } + + static bool is_nan(const StorageType & storage) + { + for (const auto & val : storage.data) + { + if (std::isnan(val)) + { + return true; + } + } + + return false; + } + + static void assign(StorageType & storage, const DataType & data_in) { storage.data = data_in; } + + static void assign(DataType & storage, const StorageType & data_in) { storage = data_in.data; } + + static void assign(StorageType & storage, const StorageType & data_in) + { + storage.data = data_in.data; + } }; + } // namespace control_toolbox #endif // CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index c6a20b88..c89b7abc 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -156,55 +156,8 @@ bool LowPassFilter::configure() return configured_ = true; } -template <> -inline bool LowPassFilter::update( - const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & 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. - if (filtered_value_.hasNaN()) - { - filtered_value_[0] = data_in.wrench.force.x; - filtered_value_[1] = data_in.wrench.force.y; - filtered_value_[2] = data_in.wrench.force.z; - filtered_value_[3] = data_in.wrench.torque.x; - filtered_value_[4] = data_in.wrench.torque.y; - filtered_value_[5] = data_in.wrench.torque.z; - filtered_old_value_ = filtered_value_; - old_value_ = filtered_value_; - } - - // IIR Filter - filtered_value_ = b1_ * old_value_ + a1_ * filtered_old_value_; - filtered_old_value_ = filtered_value_; - - // TODO(destogl): use wrenchMsgToEigen - filtered_old_value_[0] = data_in.wrench.force.x; - filtered_old_value_[1] = data_in.wrench.force.y; - filtered_old_value_[2] = data_in.wrench.force.z; - filtered_old_value_[3] = data_in.wrench.torque.x; - filtered_old_value_[4] = data_in.wrench.torque.y; - filtered_old_value_[5] = data_in.wrench.torque.z; - - data_out.wrench.force.x = filtered_value_[0]; - data_out.wrench.force.y = filtered_value_[1]; - data_out.wrench.force.z = filtered_value_[2]; - data_out.wrench.torque.x = filtered_value_[3]; - data_out.wrench.torque.y = filtered_value_[4]; - data_out.wrench.torque.z = filtered_value_[5]; - - // copy the header - data_out.header = data_in.header; - return true; -} - -template <> -inline bool LowPassFilter>::update( - const std::vector & data_in, std::vector & data_out) +template +bool LowPassFilter::update(const T & data_in, T & data_out) { if (!configured_) { @@ -212,61 +165,43 @@ 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 (filtered_value_.empty()) + if (Traits::is_nan(filtered_value_) or Traits::is_empty(filtered_value_)) { - if (std::any_of(data_in.begin(), data_in.end(), [](double val) { return !std::isfinite(val); })) + if (Traits::is_infinite(data_in)) { return false; } - filtered_value_ = data_in; - filtered_old_value_ = data_in; - old_value_ = data_in; + + Traits::assign(filtered_value_, data_in); + Traits::assign(filtered_old_value_, data_in); + Traits::assign(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])) + if constexpr (std::is_same_v>) { - old_value_[i] = data_in[i]; + 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"); } } - return true; -} + // Filter + filtered_value_ = old_value_ * b1_ + filtered_old_value_ * a1_; + filtered_old_value_ = filtered_value_; -template -bool LowPassFilter::update(const T & data_in, T & 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. - if (std::isnan(filtered_value_)) + Traits::assign(old_value_, data_in); + Traits::assign(data_out, filtered_value_); + + if (!Traits::is_infinite(data_in)) { - filtered_value_ = data_in; - filtered_old_value_ = data_in; - old_value_ = data_in; + Traits::assign(old_value_, data_in); } - // Filter - data_out = b1_ * old_value_ + a1_ * filtered_old_value_; - filtered_old_value_ = data_out; - if (std::isfinite(data_in)) + if constexpr (std::is_same_v) { - old_value_ = data_in; + data_out.header = data_in.header; } return true; From 5402d8d7e2770fa72b71ff630cc651c46011cbc2 Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Mon, 28 Apr 2025 19:58:42 +0200 Subject: [PATCH 05/12] Remove useless comments Signed-off-by: Pedro Nogueira --- .../include/control_toolbox/filter_traits.hpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp index 2ad8cb3b..5774b9d2 100644 --- a/control_toolbox/include/control_toolbox/filter_traits.hpp +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -28,12 +28,13 @@ namespace control_toolbox { -// Forward declaration of FilterTraits template struct FilterTraits; -// Wrapper around std::vector to be used as the std::vector StorageType specialization -// This is a workaround for the fact that std::vector's operator* and operator+ cannot be overloaded. +// Wrapper around std::vector to be used as +// the std::vector StorageType specialization. +// This is a workaround for the fact that +// std::vector's operator* and operator+ cannot be overloaded. struct FilterVector { std::vector data; @@ -71,7 +72,6 @@ struct FilterVector // Enable scalar * FilterVector inline FilterVector operator*(double scalar, const FilterVector & vec) { return vec * scalar; } -// Default: storage type is just T itself template struct FilterTraits { @@ -95,7 +95,6 @@ struct FilterTraits static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } }; -// Specialization: for WrenchStamped, use struct Vec6, wrapper for Eigen::Matrix template <> struct FilterTraits { @@ -152,7 +151,6 @@ struct FilterTraits static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } }; -// Specialization: for std::vector template <> struct FilterTraits> { From a21b6a2995c75c42843eb90fa96860b7771cbdd6 Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Mon, 28 Apr 2025 19:58:54 +0200 Subject: [PATCH 06/12] Fix pre commit warning Signed-off-by: Pedro Nogueira --- 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 c89b7abc..99ee7275 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -165,7 +165,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 (Traits::is_nan(filtered_value_) or Traits::is_empty(filtered_value_)) + if (Traits::is_nan(filtered_value_) || Traits::is_empty(filtered_value_)) { if (Traits::is_infinite(data_in)) { From 82da9408d70c1b7939f7d087e749a2d6c0837877 Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Mon, 28 Apr 2025 20:04:08 +0200 Subject: [PATCH 07/12] Remove useless function Signed-off-by: Pedro Nogueira --- control_toolbox/include/control_toolbox/filter_traits.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp index 5774b9d2..048a771d 100644 --- a/control_toolbox/include/control_toolbox/filter_traits.hpp +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -116,12 +116,6 @@ struct FilterTraits !std::isfinite(data.wrench.torque.y) || !std::isfinite(data.wrench.torque.z); } - static bool is_valid(const StorageType & storage) - { - return std::all_of( - storage.begin(), storage.end(), [](double val) { return std::isfinite(val); }); - } - static bool is_empty(const StorageType & storage) { (void)storage; From 45f3c747618a165538d3494b2d3f88780ff85c3b Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Wed, 30 Apr 2025 09:07:38 +0200 Subject: [PATCH 08/12] Implement generic Traits::add_metadata method Signed-off-by: Pedro Nogueira --- .../include/control_toolbox/filter_traits.hpp | 17 +++++++++++++++++ .../include/control_toolbox/low_pass_filter.hpp | 5 +---- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp index 048a771d..90df6690 100644 --- a/control_toolbox/include/control_toolbox/filter_traits.hpp +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -93,6 +93,12 @@ struct FilterTraits } static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } + + static void add_metadata(StorageType & storage, const StorageType & data_in) + { + (void)storage; + (void)data_in; + } }; template <> @@ -143,6 +149,11 @@ struct FilterTraits } static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } + + static void add_metadata(DataType & data_out, const DataType & data_in) + { + data_out.header = data_in.header; + } }; template <> @@ -191,6 +202,12 @@ struct FilterTraits> { storage.data = data_in.data; } + + static void add_metadata(DataType & storage, const DataType & data_in) + { + (void)storage; + (void)data_in; + } }; } // namespace control_toolbox diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index 99ee7275..e9f1edda 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -199,10 +199,7 @@ bool LowPassFilter::update(const T & data_in, T & data_out) Traits::assign(old_value_, data_in); } - if constexpr (std::is_same_v) - { - data_out.header = data_in.header; - } + Traits::add_metadata(data_out, data_in); return true; } From 85b46c4c77ab7f7573c0efd58f589886b7db93a7 Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Wed, 30 Apr 2025 09:08:07 +0200 Subject: [PATCH 09/12] Rename Traits::is_infinite method to Traits::is_finite Signed-off-by: Pedro Nogueira --- .../include/control_toolbox/filter_traits.hpp | 14 +++++++------- .../include/control_toolbox/low_pass_filter.hpp | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp index 90df6690..7f5d400c 100644 --- a/control_toolbox/include/control_toolbox/filter_traits.hpp +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -84,7 +84,7 @@ struct FilterTraits static bool is_nan(const StorageType & storage) { return std::isnan(storage); } - static bool is_infinite(const StorageType & storage) { return !std::isfinite(storage); } + static bool is_finite(const StorageType & storage) { return std::isfinite(storage); } static bool is_empty(const StorageType & storage) { @@ -115,11 +115,11 @@ struct FilterTraits static bool is_nan(const StorageType & storage) { return storage.hasNaN(); } - static bool is_infinite(const DataType & data) + static bool is_finite(const DataType & data) { - return !std::isfinite(data.wrench.force.x) || !std::isfinite(data.wrench.force.y) || - !std::isfinite(data.wrench.force.z) || !std::isfinite(data.wrench.torque.x) || - !std::isfinite(data.wrench.torque.y) || !std::isfinite(data.wrench.torque.z); + return std::isfinite(data.wrench.force.x) && std::isfinite(data.wrench.force.y) && + std::isfinite(data.wrench.force.z) && std::isfinite(data.wrench.torque.x) && + std::isfinite(data.wrench.torque.y) && std::isfinite(data.wrench.torque.z); } static bool is_empty(const StorageType & storage) @@ -167,13 +167,13 @@ struct FilterTraits> storage.data = std::vector{std::numeric_limits::quiet_NaN()}; } - static bool is_infinite(const StorageType & storage) + static bool is_finite(const StorageType & storage) { return std::all_of( storage.data.begin(), storage.data.end(), [](double val) { return std::isfinite(val); }); } - static bool is_infinite(const DataType & storage) + static bool is_finite(const DataType & storage) { return std::all_of( storage.begin(), storage.end(), [](double val) { return std::isfinite(val); }); diff --git a/control_toolbox/include/control_toolbox/low_pass_filter.hpp b/control_toolbox/include/control_toolbox/low_pass_filter.hpp index e9f1edda..121b6c63 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -167,7 +167,7 @@ bool LowPassFilter::update(const T & data_in, T & data_out) // so that we dont apply an impulse to the data. if (Traits::is_nan(filtered_value_) || Traits::is_empty(filtered_value_)) { - if (Traits::is_infinite(data_in)) + if (!Traits::is_finite(data_in)) { return false; } @@ -194,7 +194,7 @@ bool LowPassFilter::update(const T & data_in, T & data_out) Traits::assign(old_value_, data_in); Traits::assign(data_out, filtered_value_); - if (!Traits::is_infinite(data_in)) + if (Traits::is_finite(data_in)) { Traits::assign(old_value_, data_in); } From a1710dbca35e094797515a313dbd7cd6341ac18e Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Wed, 30 Apr 2025 09:22:40 +0200 Subject: [PATCH 10/12] Change affilliation in FilterTraits header Signed-off-by: Pedro Nogueira --- control_toolbox/include/control_toolbox/filter_traits.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp index 7f5d400c..80fe440f 100644 --- a/control_toolbox/include/control_toolbox/filter_traits.hpp +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, ros2_control development team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -104,7 +104,7 @@ struct FilterTraits template <> struct FilterTraits { - using StorageType = Eigen::Matrix; + using StorageType = Vector6d; using DataType = geometry_msgs::msg::WrenchStamped; static void initialize(StorageType & storage) From dd0e3db849ce6e9d1fbab3ab76279319e0e78230 Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Wed, 30 Apr 2025 10:06:15 +0200 Subject: [PATCH 11/12] Change Vector6d to Eigen::Matrix in FilterTraits Signed-off-by: Pedro Nogueira --- control_toolbox/include/control_toolbox/filter_traits.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/include/control_toolbox/filter_traits.hpp b/control_toolbox/include/control_toolbox/filter_traits.hpp index 80fe440f..589ddd84 100644 --- a/control_toolbox/include/control_toolbox/filter_traits.hpp +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -104,7 +104,7 @@ struct FilterTraits template <> struct FilterTraits { - using StorageType = Vector6d; + using StorageType = Eigen::Matrix; using DataType = geometry_msgs::msg::WrenchStamped; static void initialize(StorageType & storage) From e6a066e36d19becb1cfc3c5454a919a110344a5f Mon Sep 17 00:00:00 2001 From: Pedro Nogueira Date: Thu, 1 May 2025 17:58:58 +0200 Subject: [PATCH 12/12] Fix eigen matrix initialization as NaN Signed-off-by: Pedro Nogueira --- control_toolbox/include/control_toolbox/low_pass_filter.hpp | 2 -- 1 file changed, 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 121b6c63..4b112629 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -15,8 +15,6 @@ #ifndef CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ #define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ -#include - #include #include #include