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..0c6994d0 --- /dev/null +++ b/control_toolbox/include/control_toolbox/filter_traits.hpp @@ -0,0 +1,243 @@ +// 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. +// 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 +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace control_toolbox +{ + +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. +template +struct FilterVector +{ + std::vector data; + + FilterVector() = default; + + explicit FilterVector(const std::vector & vec) : data(vec) {} + + explicit FilterVector(size_t size, const U & initial_value = U{}) : data(size, initial_value) {} + + FilterVector operator*(const U & 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 +template +inline FilterVector operator*(const U & scalar, const FilterVector & vec) +{ + return vec * scalar; +} + +template +struct FilterTraits +{ + using StorageType = T; + + static void initialize(StorageType & storage) + { + storage = T{std::numeric_limits::quiet_NaN()}; + } + + static bool is_nan(const StorageType & storage) { return std::isnan(storage); } + + static bool is_finite(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; } + + static void validate_input(const T & data_in, const StorageType & filtered_value, T & data_out) + { + (void)data_in; + (void)filtered_value; + (void)data_out; // Suppress unused warnings + } + + static void add_metadata(StorageType & storage, const StorageType & data_in) + { + (void)storage; + (void)data_in; + } +}; + +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_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); + } + + 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; } + + static void validate_input( + const DataType & data_in, const StorageType & filtered_value, DataType & data_out) + { + (void)filtered_value; // Not used here + + // Compare new input's frame_id with previous output's frame_id + assert( + data_in.header.frame_id == data_out.header.frame_id && + "Frame ID changed between filter updates"); + } + + static void add_metadata(DataType & data_out, const DataType & data_in) + { + data_out.header = data_in.header; + } +}; + +template +struct FilterTraits> +{ + using StorageType = FilterVector; + using DataType = std::vector; + + static void initialize(StorageType & storage) { (void)storage; } + + static bool is_finite(const StorageType & storage) + { + return std::all_of( + storage.data.begin(), storage.data.end(), [](U val) { return std::isfinite(val); }); + } + + static bool is_finite(const DataType & storage) + { + return std::all_of(storage.begin(), storage.end(), [](U 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; + } + + static void validate_input( + const DataType & data_in, const StorageType & filtered_value, DataType & data_out) + { + assert( + data_in.size() == filtered_value.size() && + "Input vector size does not match internal state size"); + assert(data_out.size() == data_in.size() && "Input and output vectors must be the same size"); + } + + static void add_metadata(DataType & storage, const DataType & data_in) + { + (void)storage; + (void)data_in; + } +}; + +} // 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 82d40b15..f5d54c86 100644 --- a/control_toolbox/include/control_toolbox/low_pass_filter.hpp +++ b/control_toolbox/include/control_toolbox/low_pass_filter.hpp @@ -15,15 +15,16 @@ #ifndef CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ #define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ -#include - #include #include #include #include #include +#include #include +#include "control_toolbox/filter_traits.hpp" + #include "geometry_msgs/msg/wrench_stamped.hpp" namespace control_toolbox @@ -123,10 +124,13 @@ 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 Traits = FilterTraits; + using StorageType = typename Traits::StorageType; + + StorageType filtered_value_, filtered_old_value_, old_value_; + bool configured_ = false; }; @@ -143,66 +147,15 @@ 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) - { - msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = std::numeric_limits::quiet_NaN(); - } + Traits::initialize(filtered_value_); + Traits::initialize(filtered_old_value_); + Traits::initialize(old_value_); 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 (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; - 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 <> -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_) { @@ -210,62 +163,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. - // This also sets the size of the member variables to match the input data. - if (filtered_value.empty()) + if (Traits::is_nan(filtered_value_) || Traits::is_empty(filtered_value_)) { - if (std::any_of(data_in.begin(), data_in.end(), [](double val) { return !std::isfinite(val); })) + if (!Traits::is_finite(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"); + // Generic validation for all types + Traits::validate_input(data_in, filtered_value_, data_out); } - // 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]; - } - } + // Filter + filtered_value_ = old_value_ * b1_ + filtered_old_value_ * a1_; + filtered_old_value_ = filtered_value_; - return true; -} + Traits::assign(old_value_, data_in); + Traits::assign(data_out, 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)) + if (Traits::is_finite(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)) - { - old_value = data_in; - } + Traits::add_metadata(data_out, data_in); return true; }