From e05102ce496a08250a5f98cbe79806184aa5c0a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 2 Nov 2024 17:15:02 +0100 Subject: [PATCH 1/6] Update README.md (#215) --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 1e65055e..987b43bc 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,10 @@ See the documentation of [ros2_control](http://control.ros.org) and release info ## Build status ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`ros2_master`](https://github.com/ros-controls/control_toolbox/tree/ros2_master) | [![Binary Build](https://github.com/ros-controls/control_toolbox/actions/workflows/build-binary.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/build-binary.yml?branch=ros2-master)
[![Rolling Source Build](https://github.com/ros-controls/control_toolbox/actions/workflows/build-source.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/build-source.yml?branch=ros2-master) | [API](http://docs.ros.org/en/rolling/p/control_toolbox/) | [control_toolbox](https://index.ros.org/p/control_toolbox/#rolling) -**Jazzy** | [`ros2_master`](https://github.com/ros-controls/control_toolbox/tree/ros2_master) | see above | [API](http://docs.ros.org/en/jazzy/p/control_toolbox/) | [control_toolbox](https://index.ros.org/p/control_toolbox/#jazzy) -**Iron** | [`ros2_master`](https://github.com/ros-controls/control_toolbox/tree/ros2_master) | see above | [API](http://docs.ros.org/en/iron/p/control_toolbox/) | [control_toolbox](https://index.ros.org/p/control_toolbox/#iron) -**Humble** | [`ros2_master`](https://github.com/ros-controls/control_toolbox/tree/ros2_master) | see above | [API](http://docs.ros.org/en/humble/p/control_toolbox/) | [control_toolbox](https://index.ros.org/p/control_toolbox/#humble) +**Rolling** | [`ros2-master`](https://github.com/ros-controls/control_toolbox/tree/ros2-master) | [![Binary Build](https://github.com/ros-controls/control_toolbox/actions/workflows/build-binary.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/build-binary.yml?branch=ros2-master)
[![Rolling Source Build](https://github.com/ros-controls/control_toolbox/actions/workflows/build-source.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/build-source.yml?branch=ros2-master) | [API](http://docs.ros.org/en/rolling/p/control_toolbox/) | [control_toolbox](https://index.ros.org/p/control_toolbox/#rolling) +**Jazzy** | [`ros2-master`](https://github.com/ros-controls/control_toolbox/tree/ros2-master) | see above | [API](http://docs.ros.org/en/jazzy/p/control_toolbox/) | [control_toolbox](https://index.ros.org/p/control_toolbox/#jazzy) +**Iron** | [`ros2-master`](https://github.com/ros-controls/control_toolbox/tree/ros2-master) | see above | [API](http://docs.ros.org/en/iron/p/control_toolbox/) | [control_toolbox](https://index.ros.org/p/control_toolbox/#iron) +**Humble** | [`ros2-master`](https://github.com/ros-controls/control_toolbox/tree/ros2-master) | see above | [API](http://docs.ros.org/en/humble/p/control_toolbox/) | [control_toolbox](https://index.ros.org/p/control_toolbox/#humble) ## Publication From af713c0ab61b25c90c30b321ada0a8d4722f7d2d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 4 Nov 2024 21:34:05 +0100 Subject: [PATCH 2/6] Change license to Apache-2 (#220) * Change license to Apache * Add apache license badge --- LICENSE | 227 ++++++++++++++++++++++++++++++++++++++----- LICENSE.BSD-3-clause | 25 +++++ README.md | 1 + package.xml | 7 +- 4 files changed, 233 insertions(+), 27 deletions(-) create mode 100644 LICENSE.BSD-3-clause diff --git a/LICENSE b/LICENSE index 574ef079..d6456956 100644 --- a/LICENSE +++ b/LICENSE @@ -1,25 +1,202 @@ -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - - * Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/LICENSE.BSD-3-clause b/LICENSE.BSD-3-clause new file mode 100644 index 00000000..574ef079 --- /dev/null +++ b/LICENSE.BSD-3-clause @@ -0,0 +1,25 @@ +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md index 987b43bc..8736c6c5 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ control_toolbox =========== [![License](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) +[![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![codecov](https://codecov.io/gh/ros-controls/control_toolbox/graph/badge.svg?token=0o4dFzADHj)](https://codecov.io/gh/ros-controls/control_toolbox) This package contains several C++ classes useful in writing controllers. diff --git a/package.xml b/package.xml index 6e183076..b055c584 100644 --- a/package.xml +++ b/package.xml @@ -3,10 +3,13 @@ control_toolbox 3.3.0 The control toolbox contains modules that are useful across all controllers. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota - BSD-3-Clause + Apache License 2.0 https://control.ros.org https://github.com/ros-controls/control_toolbox/issues From 9d8d2906ab80b630edf510b735460c9ab923314e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 6 Nov 2024 20:14:10 +0100 Subject: [PATCH 3/6] Pid class does not depend on rclcpp (#221) --- include/control_toolbox/pid.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 877edb9f..8f3272a7 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -33,15 +33,12 @@ #ifndef CONTROL_TOOLBOX__PID_HPP_ #define CONTROL_TOOLBOX__PID_HPP_ +#include #include +#include #include -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/node.hpp" - #include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" #include "control_toolbox/visibility_control.hpp" From 7d008a41c50ac75bd5e0a29723305e5df00ed25e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 6 Nov 2024 20:14:31 +0100 Subject: [PATCH 4/6] Add standalone version of LPF (#222) --------- Co-authored-by: Patrick Roncagliolo --- include/control_filters/low_pass_filter.hpp | 107 ++------- include/control_toolbox/low_pass_filter.hpp | 224 ++++++++++++++++++ .../test_load_low_pass_filter.cpp | 9 +- 3 files changed, 252 insertions(+), 88 deletions(-) create mode 100644 include/control_toolbox/low_pass_filter.hpp diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index e4a11782..830dbb0d 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -21,11 +21,12 @@ #include #include -#include "low_pass_filter_parameters.hpp" #include "filters/filter_base.hpp" - #include "geometry_msgs/msg/wrench_stamped.hpp" +#include "control_toolbox/low_pass_filter.hpp" +#include "low_pass_filter_parameters.hpp" + namespace control_filters { @@ -79,14 +80,6 @@ template class LowPassFilter : public filters::FilterBase { public: - // Default constructor - LowPassFilter(); - - /*! - * \brief Destructor of LowPassFilter class. - */ - ~LowPassFilter() override; - /*! * \brief Configure the LowPassFilter (access and process params). */ @@ -102,44 +95,14 @@ class LowPassFilter : public filters::FilterBase */ bool update(const T & data_in, T & data_out) override; -protected: - /*! - * \brief Internal computation of the feedforward and feedbackward coefficients - * according to the LowPassFilter parameters. - */ - void compute_internal_params() - { - a1_ = exp( - -1.0 / parameters_.sampling_frequency * (2.0 * M_PI * parameters_.damping_frequency) / - (pow(10.0, parameters_.damping_intensity / -10.0))); - b1_ = 1.0 - a1_; - }; - private: rclcpp::Clock::SharedPtr clock_; std::shared_ptr logger_; std::shared_ptr parameter_handler_; low_pass_filter::Params parameters_; - - // Filter parameters - /** internal data storage (double). */ - double filtered_value, filtered_old_value, old_value; - /** internal data storage (wrench). */ - Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; - double a1_; /**< feedbackward coefficient. */ - double b1_; /**< feedforward coefficient. */ + std::shared_ptr> lpf_; }; -template -LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) -{ -} - -template -LowPassFilter::~LowPassFilter() -{ -} - template bool LowPassFilter::configure() { @@ -168,24 +131,19 @@ bool LowPassFilter::configure() } } parameters_ = parameter_handler_->get_params(); - compute_internal_params(); + lpf_ = std::make_shared>( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); - // Initialize storage Vectors - filtered_value = filtered_old_value = old_value = 0; - // TODO(destogl): make the size parameterizable and more intelligent is using complex types - for (size_t i = 0; i < 6; ++i) - { - msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; - } - - return true; + return lpf_->configure(); } template <> inline bool LowPassFilter::update( const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!this->configured_) + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { if (logger_) RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); @@ -196,39 +154,22 @@ inline bool LowPassFilter::update( if (parameter_handler_->is_old(parameters_)) { parameters_ = parameter_handler_->get_params(); - compute_internal_params(); + lpf_->set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); } - // 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; + return lpf_->update(data_in, data_out); } template bool LowPassFilter::update(const T & data_in, T & data_out) { - if (!this->configured_) + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + if (logger_) + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } @@ -236,15 +177,13 @@ bool LowPassFilter::update(const T & data_in, T & data_out) if (parameter_handler_->is_old(parameters_)) { parameters_ = parameter_handler_->get_params(); - compute_internal_params(); + lpf_->set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); } - // Filter - data_out = b1_ * old_value + a1_ * filtered_old_value; - filtered_old_value = data_out; - old_value = data_in; - - return true; + return lpf_->update(data_in, data_out); } } // namespace control_filters diff --git a/include/control_toolbox/low_pass_filter.hpp b/include/control_toolbox/low_pass_filter.hpp new file mode 100644 index 00000000..df214d09 --- /dev/null +++ b/include/control_toolbox/low_pass_filter.hpp @@ -0,0 +1,224 @@ +// 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__LOW_PASS_FILTER_HPP_ +#define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace control_toolbox +{ + +/***************************************************/ +/*! \class LowPassFilter + \brief A Low-pass filter class. + + This class implements a low-pass filter for + various data types based on an Infinite Impulse Response Filter. + For vector elements, the filtering is applied separately on + each element of the vector. + + In particular, this class implements a simplified version of + an IIR filter equation : + + \f$y(n) = b x(n-1) + a y(n-1)\f$ + + where:
+
    +
  • \f$ x(n)\f$ is the input signal +
  • \f$ y(n)\f$ is the output signal (filtered) +
  • \f$ b \f$ is the feedforward filter coefficient +
  • \f$ a \f$ is the feedback filter coefficient +
+ + and the Low-Pass coefficient equation: +
+
    +
  • \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$ +
  • \f$ b = 1 - a \f$ +
+ + where:
+
    +
  • \f$ sf \f$ is the sampling frequency +
  • \f$ df \f$ is the damping frequency +
  • \f$ di \f$ is the damping intensity (amplitude) +
+ + \section Usage + + For manual instantiation, you should first call configure() + (in non-realtime) and then call update() at every update step. + +*/ +/***************************************************/ + +template +class LowPassFilter +{ +public: + // Default constructor + LowPassFilter(); + + LowPassFilter(double sampling_frequency, double damping_frequency, double damping_intensity){ + set_params(sampling_frequency, damping_frequency, damping_intensity); + } + + /*! + * \brief Destructor of LowPassFilter class. + */ + ~LowPassFilter(); + + /*! + * \brief Configure the LowPassFilter (access and process params). + */ + bool configure(); + + /*! + * \brief Applies one iteration of the IIR filter. + * + * \param data_in input to the filter + * \param data_out filtered output + * + * \returns false if filter is not configured, true otherwise + */ + bool update(const T & data_in, T & data_out); + + bool set_params( + const double sampling_frequency, + const double damping_frequency, + const double damping_intensity) + { + // TODO(roncapat): parameters validation + this->sampling_frequency = sampling_frequency; + this->damping_frequency = damping_frequency; + this->damping_intensity = damping_intensity; + compute_internal_params(); + return true; + } + + bool is_configured() const + { + return configured_; + } + +protected: + /*! + * \brief Internal computation of the feedforward and feedbackward coefficients + * according to the LowPassFilter parameters. + */ + void compute_internal_params() + { + a1_ = exp( + -1.0 / sampling_frequency * (2.0 * M_PI * damping_frequency) / + (pow(10.0, damping_intensity / -10.0))); + b1_ = 1.0 - a1_; + }; + +private: + // Filter parameters + /** internal data storage (double). */ + double filtered_value, filtered_old_value, old_value; + /** internal data storage (wrench). */ + Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + double sampling_frequency, damping_frequency, damping_intensity; + double a1_; /** feedbackward coefficient. */ + double b1_; /** feedforward coefficient. */ + bool configured_ = false; +}; + +template +LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) +{ +} + +template +LowPassFilter::~LowPassFilter() +{ +} + +template +bool LowPassFilter::configure() +{ + compute_internal_params(); + + // Initialize storage Vectors + filtered_value = filtered_old_value = old_value = 0; + // TODO(destogl): make the size parameterizable and more intelligent is using complex types + for (size_t i = 0; i < 6; ++i) + { + msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; + } + + return configured_ = true; +} + +template <> +inline bool LowPassFilter::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) +{ + if (!configured_) + { + return false; + } + + // 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 +bool LowPassFilter::update(const T & data_in, T & data_out) +{ + if (!configured_) + { + return false; + } + + // Filter + data_out = b1_ * old_value + a1_ * filtered_old_value; + filtered_old_value = data_out; + old_value = data_in; + + return true; +} + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp index f3c00fa8..91eb3568 100644 --- a/test/control_filters/test_load_low_pass_filter.cpp +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -15,11 +15,12 @@ #include #include #include -#include "control_filters/low_pass_filter.hpp" + #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/utilities.hpp" -#include +#include "pluginlib/class_loader.hpp" +#include "control_filters/low_pass_filter.hpp" TEST(TestLoadLowPassFilter, load_low_pass_filter_double) { @@ -38,7 +39,7 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_double) std::string filter_type = "control_filters/LowPassFilterDouble"; ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); - ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); rclcpp::shutdown(); } @@ -60,7 +61,7 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) std::string filter_type = "control_filters/LowPassFilterWrench"; ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); - ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); rclcpp::shutdown(); } From 795b9e69a05ff595fb0a0e39adf44c299db3b755 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 6 Nov 2024 22:16:02 +0100 Subject: [PATCH 5/6] LPF: Throw if calling `udpate` unconfigured (#229) --- include/control_filters/low_pass_filter.hpp | 8 ++--- include/control_toolbox/low_pass_filter.hpp | 29 +++++-------------- test/control_filters/test_low_pass_filter.cpp | 21 +++++++++++--- .../test_low_pass_filter_parameters.yaml | 2 +- 4 files changed, 27 insertions(+), 33 deletions(-) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 830dbb0d..4601076c 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -145,9 +145,7 @@ inline bool LowPassFilter::update( { if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { - if (logger_) - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); - return false; + throw std::runtime_error("Filter is not configured"); } // Update internal parameters if required @@ -168,9 +166,7 @@ bool LowPassFilter::update(const T & data_in, T & data_out) { if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { - if (logger_) - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); - return false; + throw std::runtime_error("Filter is not configured"); } // Update internal parameters if required diff --git a/include/control_toolbox/low_pass_filter.hpp b/include/control_toolbox/low_pass_filter.hpp index df214d09..07351bdd 100644 --- a/include/control_toolbox/low_pass_filter.hpp +++ b/include/control_toolbox/low_pass_filter.hpp @@ -16,8 +16,10 @@ #define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ #include + #include #include +#include #include #include @@ -101,30 +103,16 @@ class LowPassFilter */ bool update(const T & data_in, T & data_out); - bool set_params( - const double sampling_frequency, - const double damping_frequency, - const double damping_intensity) - { - // TODO(roncapat): parameters validation - this->sampling_frequency = sampling_frequency; - this->damping_frequency = damping_frequency; - this->damping_intensity = damping_intensity; - compute_internal_params(); - return true; - } - bool is_configured() const { return configured_; } -protected: /*! * \brief Internal computation of the feedforward and feedbackward coefficients * according to the LowPassFilter parameters. */ - void compute_internal_params() + void set_params(double sampling_frequency, double damping_frequency, double damping_intensity) { a1_ = exp( -1.0 / sampling_frequency * (2.0 * M_PI * damping_frequency) / @@ -134,13 +122,12 @@ class LowPassFilter private: // 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 (wrench). */ Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; - double sampling_frequency, damping_frequency, damping_intensity; - double a1_; /** feedbackward coefficient. */ - double b1_; /** feedforward coefficient. */ bool configured_ = false; }; @@ -157,8 +144,6 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - compute_internal_params(); - // Initialize storage Vectors filtered_value = filtered_old_value = old_value = 0; // TODO(destogl): make the size parameterizable and more intelligent is using complex types @@ -176,7 +161,7 @@ inline bool LowPassFilter::update( { if (!configured_) { - return false; + throw std::runtime_error("Filter is not configured"); } // IIR Filter @@ -208,7 +193,7 @@ bool LowPassFilter::update(const T & data_in, T & data_out) { if (!configured_) { - return false; + throw std::runtime_error("Filter is not configured"); } // Filter diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index 4f2cc9ce..fcb76ca0 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -57,7 +57,23 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter) node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } -TEST_F(LowPassFilterTest, TestLowPassFilterComputation) +TEST_F(LowPassFilterTest, TestLowPassFilterThrowsUnconfigured) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + double in, out; + ASSERT_THROW(filter_->update(in, out), std::runtime_error); +} + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterThrowsUnconfigured) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + geometry_msgs::msg::WrenchStamped in, out; + ASSERT_THROW(filter_->update(in, out), std::runtime_error); +} + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterComputation) { // parameters should match the test yaml file double sampling_freq = 1000.0; @@ -77,9 +93,6 @@ TEST_F(LowPassFilterTest, TestLowPassFilterComputation) std::shared_ptr> filter_ = std::make_shared>(); - // not yet configured, should deny update - ASSERT_FALSE(filter_->update(in, out)); - // configure ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); diff --git a/test/control_filters/test_low_pass_filter_parameters.yaml b/test/control_filters/test_low_pass_filter_parameters.yaml index 11e08cc1..d220da10 100644 --- a/test/control_filters/test_low_pass_filter_parameters.yaml +++ b/test/control_filters/test_low_pass_filter_parameters.yaml @@ -15,7 +15,7 @@ TestLowPassWrenchFilterInvalidThenFixedParameter: damping_frequency: 20.5 damping_intensity: 1.25 -TestLowPassFilterComputation: +TestLowPassWrenchFilterComputation: ros__parameters: sampling_frequency: 1000.0 damping_frequency: 20.5 From 7dda65fb78ac538d27a9346423b3f6b9a0347798 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 6 Nov 2024 23:00:17 +0100 Subject: [PATCH 6/6] Add the same compile flags as with ros2_controllers and fix errors (#219) --- CMakeLists.txt | 3 ++- src/limited_proxy.cpp | 18 +++++++++--------- src/pid.cpp | 4 ++-- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9786821c..8777918d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(control_toolbox LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() if(WIN32) diff --git a/src/limited_proxy.cpp b/src/limited_proxy.cpp index ecb11fa4..15669f1b 100644 --- a/src/limited_proxy.cpp +++ b/src/limited_proxy.cpp @@ -144,15 +144,15 @@ double LimitedProxy::update( { // Get the parameters. This ensures that they can not change during // the calculations and are non-negative! - double mass = abs(mass_); // Estimate of the joint mass - double Kd = abs(Kd_); // Damping gain - double Kp = abs(Kp_); // Position gain - double Ki = abs(Ki_); // Integral gain - double Ficl = abs(Ficl_); // Integral force clamp - double Flim = abs(effort_limit_); // Limit on output force - double vlim = abs(vel_limit_); // Limit on velocity - double lam = abs(lambda_proxy_); // Bandwidth of proxy reconvergence - double acon = abs(acc_converge_); // Acceleration of proxy reconvergence + double mass = std::abs(mass_); // Estimate of the joint mass + double Kd = std::abs(Kd_); // Damping gain + double Kp = std::abs(Kp_); // Position gain + double Ki = std::abs(Ki_); // Integral gain + double Ficl = std::abs(Ficl_); // Integral force clamp + double Flim = std::abs(effort_limit_); // Limit on output force + double vlim = std::abs(vel_limit_); // Limit on velocity + double lam = std::abs(lambda_proxy_); // Bandwidth of proxy reconvergence + double acon = std::abs(acc_converge_); // Acceleration of proxy reconvergence // For numerical stability, upper bound the bandwidth by 2/dt. // Note this is safe for dt==0. diff --git a/src/pid.cpp b/src/pid.cpp index 282aafb2..c1f23a12 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -130,7 +130,7 @@ double Pid::computeCommand(double error, uint64_t dt) error_dot_ = d_error_; // Calculate the derivative error - error_dot_ = (error - p_error_last_) / (dt / 1e9); + error_dot_ = (error - p_error_last_) / (static_cast(dt) / 1e9); p_error_last_ = error; return computeCommand(error, error_dot_, dt); @@ -155,7 +155,7 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt) p_term = gains.p_gain_ * p_error_; // Calculate the integral of the position error - i_error_ += (dt / 1e9) * p_error_; + i_error_ += (static_cast(dt) / 1e9) * p_error_; if (gains.antiwindup_ && gains.i_gain_ != 0) { // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_