diff --git a/CHANGELOG.md b/CHANGELOG.md index 9b793149a3..5f4cf2a853 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,10 +13,11 @@ All notable changes to this project are documented in this file. - Add getJointTorques to python binding in SensorBridge (https://github.com/ami-iit/bipedal-locomotion-framework/pull/825) - Add `System::TimeProfiler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/826) - Add the possibility to programmatically build a `QPTSID` problem from the content of a `ParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/828) -- Add support for testing if a portion of code allocates memory via `MemoryAllocationMonitor`` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/768) +- Add support for testing if a portion of code allocates memory via `MemoryAllocationMonitor` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/768) - Implement `Math::ZeroOrderSpline` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/834) - Add the possibility to get only position or position/velocity from the spline (https://github.com/ami-iit/bipedal-locomotion-framework/pull/834) - Add the possibility to set the number of threads used by onnxruntime in `MANN` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/836) +- Implement `ButterworthLowPassFilter` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/838) ### Changed - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810) diff --git a/src/ContinuousDynamicalSystem/CMakeLists.txt b/src/ContinuousDynamicalSystem/CMakeLists.txt index f8d43b6446..ee58d77619 100644 --- a/src/ContinuousDynamicalSystem/CMakeLists.txt +++ b/src/ContinuousDynamicalSystem/CMakeLists.txt @@ -17,10 +17,12 @@ if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem) ${H_PREFIX}/Integrator.h ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/ForwardEuler.h ${H_PREFIX}/RK4.h ${H_PREFIX}/CompliantContactWrench.h ${H_PREFIX}/MultiStateWeightProvider.h + ${H_PREFIX}/ButterworthLowPassFilter.h PRIVATE_HEADERS ${H_PREFIX}/impl/traits.h SOURCES src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CompliantContactWrench.cpp src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp src/CentroidalDynamics.cpp src/FirstOrderSmoother.cpp src/MultiStateWeightProvider.cpp + src/ButterworthLowPassFilter.cpp PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ContactModels BipedalLocomotion::System iDynTree::idyntree-high-level iDynTree::idyntree-model Eigen3::Eigen BipedalLocomotion::TextLogging BipedalLocomotion::Math BipedalLocomotion::Contacts diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ButterworthLowPassFilter.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ButterworthLowPassFilter.h new file mode 100644 index 0000000000..2f87ff4c0f --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ButterworthLowPassFilter.h @@ -0,0 +1,165 @@ +/** + * @file ButterworthLowPassFilter.h + * @authors Giulio Romualdi + * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_BUTTERWORTH_LOW_PASS_FILTER_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_BUTTERWORTH_LOW_PASS_FILTER_H + +#include + +#include +#include +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * ButterworthLowPass implements a low pass filter of order N. + * + * The system is described by the following transfer function + * \f[ + * H(s) = \frac{1}{\sqrt{1 + \left(\frac{s}{\omega_c}\right)^{2N}}} + * \f] + * where \f$\omega_c\f$ is the cutoff frequency and \f$N\f$ is the order of the filter and \f$s\f$ + * is the Laplace variable. + * + * To compute the coefficient of the filter we split the problem in three steps: + * -# Compute the transfer function of the continuous system. + * -# Compute the transfer function of the discrete system. + * -# Compute the coefficients of the discrete system. + * + * @section Compute the transfer function of the continuous system + * What follows is taken from from Passive and Active Network Analysis and Synthesis, Aram Budak, + * Houghton Mifflin, 1974 and from + * https://dsp.stackexchange.com/questions/79498/butterworth-filter-poles + * The poles of the Butterworth filter are evenly spaced on a circle of radius \f$\omega_c\f$ in + * the s-plane. The poles are given by + * \f[ + * p_k = \omega_c e^{j \frac{\pi}{2} \left(1 + \frac{2k - 1}{2N}\right)} + * \f] + * where \f$k = 0, 1, \ldots, N-1\f$ and \f$j\f$ is the imaginary unit. + * By construction, the Butterworth filter does not have zeros. + * The gain of the filter is given by + * \f[ + * K = \prod_{k=0}^{N-1} s_k = \omega_c^N + * \f] + * @section Compute the transfer function of the discrete system + * As mentioned before, the transfer function of the discrete system is obtained by the bilinear + * transform + * \f[ + * s = \frac{2}{\delta t} \frac{1 - z^{-1}}{1 + z^{-1}} + * \f] + * The poles of the discrete system are obtained by substituting the poles of the continuous system + * in the bilinear transformation as explained in + * https://it.mathworks.com/help/signal/ref/bilinear.html + * The poles of the discrete system are given by + * \f[ + * p^d_k = \frac{1 + p_k \delta t/2}{1 - p_k \delta t/2} + * \f] + * where \f$p_k\f$ are the poles of the continuous system, \f$\delta t\f$ is the sampling time and + * \f$k = 0, 1, \ldots, N-1\f$. + * All the zeros of the continuous system are mapped to -1. + * Finally, the gain of the discrete system is given by + * \f[ + * K^d = \text{real} \frac{K}{ \prod (\frac{2}{\delta T} - p_k) } + * \f] + * @section Compute the coefficients of the discrete system + * Once we have the poles and the gain of the discrete system we can compute the coefficients of the + * filter by applying the [Vieta's formulas](https://en.wikipedia.org/wiki/Vieta%27s_formulas). + * The transfer function of the discrete system is given by + * \f[ + * H(z) = \frac{a_n + a_{n-1} z^{-1} + \ldots + a_1 z^{-n+1} + a_0 z^{-n}}{1 + b_{n-1} z^{-1} + \ldots + b_1 z^{-n+1} + b_0 z^{-n}} + * \f] + * Once the numerator and the denominator are computed we can easily antitransform the transfer function + * to obtain the coefficients of the filter as + * \[ + * y[k] = \frac{1}{b_0} \left( a_0 x[k] + a_1 x[k-1] + \ldots + a_n x[k-n] - b_1 y[k-1] - \ldots - b_n y[k-n] \right) + * \] + * where \f$x[k]\f$ is the input of the filter and \f$y[k]\f$ is the output of the filter. + */ +class ButterworthLowPassFilter + : public BipedalLocomotion::System::Advanceable +{ +public: + /** + * Constructor + */ + ButterworthLowPassFilter(); + + /** + * Destructor + */ + ~ButterworthLowPassFilter() override; + + // clang-format off + /** + * Initialize the Dynamical system. + * @param handler pointer to the parameter handler. + * @note the following parameters are required + * | Parameter Name | Type | Description | Mandatory | + * |:------------------:|:--------:|:------------------------------------------------:|:---------:| + * | `sampling_time` | `double` | Sampling time used to discrete the linear system | Yes | + * | `cutoff_frequency`| `double` | Cutoff frequency of the low pass filter. | Yes | + * | `order` | `int` | Order of the low pass filter. | Yes | + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler) override; + // clang-format on + + /** + * Set the state of the smoother. + * @param initialState initial state of the smoother + * @return true in case of success, false otherwise. + */ + bool reset(Eigen::Ref initialState); + + /** + * Compute the output of the filter. + * @return true in case of success, false otherwise. + * @note The function update also the internal state of the filter. + */ + bool advance() override; + + /** + * Get the output of the filter. + * @return a vector containing the output of the smoother + */ + const Eigen::VectorXd& getOutput() const override; + + /** + * Set the input of the filter + * @param input the vector representing the input of the filter + * @return True in case of success and false otherwise + */ + bool setInput(const Eigen::VectorXd& input) override; + + /** + * Set the input of the filter + * @param input the vector representing the input of the filter + * @return True in case of success and false otherwise + */ + bool setInput(Eigen::Ref input); + + /** + * Determines the validity of the object retrieved with getOutput() + * @return True if the object is valid, false otherwise. + */ + bool isOutputValid() const override; + +private: + struct Impl; /**< Pimpl idiom */ + std::unique_ptr m_pimpl; /**< Pointer to the implementation */ +}; + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_BUTTERWORTH_LOW_PASS_FILTER_H diff --git a/src/ContinuousDynamicalSystem/src/ButterworthLowPassFilter.cpp b/src/ContinuousDynamicalSystem/src/ButterworthLowPassFilter.cpp new file mode 100644 index 0000000000..0cb489ed68 --- /dev/null +++ b/src/ContinuousDynamicalSystem/src/ButterworthLowPassFilter.cpp @@ -0,0 +1,257 @@ +/** + * @file ButterworthLowPassFilter.cpp + * @authors Giulio Romualdi + * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ContinuousDynamicalSystem; + +struct ButterworthLowPassFilter::Impl +{ + double frequency{0.0}; + double cutOffPulsation{0.0}; + int order{0}; + + std::vector> continuousPoles; + double continuousGain{0.0}; + + std::vector> discreteZeros; + std::vector> discretePoles; + double discreteGain{0.0}; + + std::vector numerator; /**< Numerator of the discrete transfer function. The first + element is the highest order coefficient. */ + std::vector denominator; /**< Denominator of the discrete transfer function. The first + element is the highest order coefficient. */ + + std::deque inputBuffer; + std::deque outputBuffer; + + Eigen::VectorXd output; + + bool isOutputValid{false}; + + void computePole() + { + this->continuousPoles.clear(); + for (int i = 0; i < this->order; i++) + { + using namespace std::complex_literals; + // equation taken from + std::complex pole + = this->cutOffPulsation + * std::exp(1i * M_PI_2 * (1.0 + (1.0 + 2.0 * i) / double(this->order))); + this->continuousPoles.push_back(pole); + } + + // the gain is computed as the product of the poles + std::complex tmp = 1.0; + for (const auto& pole : this->continuousPoles) + { + tmp *= pole; + } + this->continuousGain = this->order % 2 == 0 ? tmp.real() : -tmp.real(); + } + + void bilinearTransformation() + { + const double k = 2 * this->frequency; + // bilinear transformation + for (const auto& pole : this->continuousPoles) + { + std::complex z = (1.0 + pole / k) / (1.0 - pole / k); + this->discretePoles.push_back(z); + } + + this->discreteZeros = std::vector>(this->discretePoles.size(), -1.0); + + std::complex tmp = 1.0; + for (const auto& pole : this->continuousPoles) + { + tmp *= (k - pole); + } + + this->discreteGain = (this->continuousGain / tmp).real(); + } + + std::vector computeVietaFormula(const std::vector>& roots) + { + int n = roots.size(); + std::vector> coeffs(n + 1, 0.0); + coeffs[0] = 1; // Leading coefficient is always 1 + for (int i = 0; i < n; ++i) + { + for (int j = n; j > 0; --j) + { + coeffs[j] = coeffs[j] - roots[i] * coeffs[j - 1]; + } + } + + std::vector coefficients(n + 1); + for (int i = 0; i < n + 1; i++) + { + coefficients[i] = coeffs[i].real(); + } + + return coefficients; + } +}; + +ButterworthLowPassFilter::ButterworthLowPassFilter() +{ + this->m_pimpl = std::make_unique(); +} + +ButterworthLowPassFilter::~ButterworthLowPassFilter() = default; + +bool ButterworthLowPassFilter::initialize( + std::weak_ptr handler) +{ + constexpr auto logPrefix = "[ButterworthLowPassFilter::initialize]"; + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} Invalid parameter handler.", logPrefix); + return false; + } + + std::chrono::nanoseconds samplingTime; + if (!ptr->getParameter("sampling_time", samplingTime)) + { + log()->error("{} Unable to get the parameter named 'sampling_time'.", logPrefix); + return false; + } + m_pimpl->frequency = 1.0 / std::chrono::duration(samplingTime).count(); + + double cutOffFrequency; + if (!ptr->getParameter("cutoff_frequency", cutOffFrequency)) + { + log()->error("{} Unable to get the parameter named 'cutoff_frequency'.", logPrefix); + return false; + } + m_pimpl->cutOffPulsation = 2 * M_PI * cutOffFrequency; + + if (!ptr->getParameter("order", m_pimpl->order)) + { + log()->error("{} Unable to get the parameter named 'order'.", logPrefix); + return false; + } + + // compute the poles and the gain of the continuous system + m_pimpl->computePole(); + + // apply the bilinear transformation + m_pimpl->bilinearTransformation(); + + // compute the coefficients of the discrete system + m_pimpl->numerator = m_pimpl->computeVietaFormula(m_pimpl->discreteZeros); + for (auto& element : m_pimpl->numerator) + { + element *= m_pimpl->discreteGain; + } + m_pimpl->denominator = m_pimpl->computeVietaFormula(m_pimpl->discretePoles); + + return true; +} + +bool ButterworthLowPassFilter::reset(Eigen::Ref initialState) +{ + m_pimpl->inputBuffer.clear(); + m_pimpl->outputBuffer.clear(); + + // initialize the input buffer + for (int i = 0; i < m_pimpl->order + 1; i++) + { + m_pimpl->inputBuffer.push_back(initialState); + } + for (int i = 0; i < m_pimpl->order; i++) + { + m_pimpl->outputBuffer.push_back(initialState); + } + + m_pimpl->isOutputValid = false; + + return true; +} + +bool ButterworthLowPassFilter::setInput(Eigen::Ref input) +{ + constexpr auto logPrefix = "[ButterworthLowPassFilter::setInput]"; + if (m_pimpl->inputBuffer.empty() || m_pimpl->outputBuffer.empty()) + { + log()->error("{} The filter has not been reset.", logPrefix); + return false; + } + + if (input.size() != m_pimpl->inputBuffer.front().size()) + { + log()->error("{} The input has a wrong size.", logPrefix); + return false; + } + + m_pimpl->inputBuffer.push_front(input); + m_pimpl->inputBuffer.pop_back(); + + m_pimpl->isOutputValid = false; + return true; +} + +bool ButterworthLowPassFilter::setInput(const Eigen::VectorXd& input) +{ + return this->setInput(Eigen::Ref(input)); +} + +bool ButterworthLowPassFilter::advance() +{ + constexpr auto logPrefix = "[ButterworthLowPassFilter::advance]"; + m_pimpl->isOutputValid = false; + + if (m_pimpl->inputBuffer.size() != m_pimpl->order + 1) + { + log()->error("{} The input buffer has a wrong size.", logPrefix); + return false; + } + + if (m_pimpl->outputBuffer.size() != m_pimpl->order) + { + log()->error("{} The output buffer has a wrong size.", logPrefix); + return false; + } + + m_pimpl->output = Eigen::VectorXd::Zero(m_pimpl->inputBuffer.front().size()); + for (int i = 0; i < m_pimpl->order + 1; i++) + { + m_pimpl->output += m_pimpl->numerator[i] * m_pimpl->inputBuffer[i]; + } + + for (int i = 0; i < m_pimpl->outputBuffer.size(); i++) + { + m_pimpl->output -= m_pimpl->denominator[i + 1] * m_pimpl->outputBuffer[i]; + } + + m_pimpl->output /= m_pimpl->denominator[0]; + + m_pimpl->outputBuffer.push_front(m_pimpl->output); + m_pimpl->outputBuffer.pop_back(); + + m_pimpl->isOutputValid = true; + + return true; +} + +const Eigen::VectorXd& ButterworthLowPassFilter::getOutput() const +{ + return m_pimpl->output; +} + +bool ButterworthLowPassFilter::isOutputValid() const +{ + return m_pimpl->isOutputValid; +} diff --git a/src/ContinuousDynamicalSystem/tests/ButterworthLowPassFilter.cpp b/src/ContinuousDynamicalSystem/tests/ButterworthLowPassFilter.cpp new file mode 100644 index 0000000000..0c734d02bb --- /dev/null +++ b/src/ContinuousDynamicalSystem/tests/ButterworthLowPassFilter.cpp @@ -0,0 +1,49 @@ +/** + * @file ButterworthLowPassFilter.cpp + * @authors Giulio Romualdi + * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include + +// Catch2 +#include + +#include + +#include +#include + +using namespace BipedalLocomotion::ContinuousDynamicalSystem; + +TEST_CASE("ButterworthLowPass filter") +{ + using namespace std::chrono_literals; + constexpr int order = 20; + constexpr double cutOffFrequency = 10.0; + constexpr std::chrono::nanoseconds dT = 10ms; + + auto paramHandler = std::make_shared(); + paramHandler->setParameter("order", order); + paramHandler->setParameter("cutoff_frequency", cutOffFrequency); + paramHandler->setParameter("sampling_time", dT); + + ButterworthLowPassFilter butterworthLowPass; + REQUIRE(butterworthLowPass.initialize(paramHandler)); + + Eigen::VectorXd input = Eigen::VectorXd::Zero(2); + + REQUIRE(butterworthLowPass.reset(input)); + + input << 1.0, 1.0; + + constexpr std::chrono::nanoseconds simulationTime = 2s; + for (std::chrono::nanoseconds t = 0ns; t < simulationTime; t += dT) + { + REQUIRE(butterworthLowPass.setInput(input)); + REQUIRE(butterworthLowPass.advance()); + + std::cout << butterworthLowPass.getOutput().transpose() << std::endl; + } +} diff --git a/src/ContinuousDynamicalSystem/tests/CMakeLists.txt b/src/ContinuousDynamicalSystem/tests/CMakeLists.txt index f80cb30feb..7fe1ad44dc 100644 --- a/src/ContinuousDynamicalSystem/tests/CMakeLists.txt +++ b/src/ContinuousDynamicalSystem/tests/CMakeLists.txt @@ -26,3 +26,9 @@ add_bipedal_test( SOURCES MultiStateWeightProvider.cpp LINKS BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::TestUtils Eigen3::Eigen) + +add_bipedal_test( + NAME ButterworthLowPassFilterTest + SOURCES ButterworthLowPassFilter.cpp + LINKS BipedalLocomotion::ContinuousDynamicalSystem + Eigen3::Eigen)