From e8794ee6b5c5c0b13472973a86e42d37d0c44f48 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 12 Jun 2021 17:05:10 +0200 Subject: [PATCH 01/13] Added initial files of control filters --- CMakeLists.txt | 60 ++++- README.md | 22 ++ control_filters.xml | 13 + .../control_filters/gravity_compensation.hpp | 222 ++++++++++++++++++ package.xml | 5 + src/control_filters/gravity_compensation.cpp | 20 ++ .../test_gravity_compensation.cpp | 42 ++++ .../test_gravity_compensation.hpp | 59 +++++ .../test_load_gravity_compensation.cpp | 41 ++++ 9 files changed, 481 insertions(+), 3 deletions(-) create mode 100644 control_filters.xml create mode 100644 include/control_filters/gravity_compensation.hpp create mode 100644 src/control_filters/gravity_compensation.cpp create mode 100644 test/control_filters/test_gravity_compensation.cpp create mode 100644 test/control_filters/test_gravity_compensation.hpp create mode 100644 test/control_filters/test_load_gravity_compensation.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e8043922..942d4b05 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,38 @@ ament_target_dependencies(${PROJECT_NAME} rcutils realtime_tools) +######################## +# Build control filters +######################## +find_package(filters REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +set(CONTROL_FILTERS_INCLUDE_DEPENDS + filters + geometry_msgs + pluginlib + rclcpp + tf2_geometry_msgs + tf2_ros +) + +add_library(gravity_compensation SHARED + src/control_filters/gravity_compensation.cpp +) +target_include_directories(gravity_compensation PUBLIC + $ + $ +) +ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + +########## +# Testing +########## if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) @@ -57,6 +89,12 @@ if(BUILD_TESTING) ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) target_link_libraries(pid_publisher_tests ${PROJECT_NAME}) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) + + ## Control Filters + ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) + target_link_libraries(test_gravity_compensation gravity_compensation) + ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + endif() # Install @@ -68,11 +106,27 @@ install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +install(TARGETS gravity_compensation + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install pluginlib xml +pluginlib_export_plugin_description_file(filters control_filters.xml) + ament_export_dependencies( rclcpp rcutils - realtime_tools) -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) + realtime_tools + ${CONTROL_FILTERS_INCLUDE_DEPENDS} +) +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} + gravity_compensation +) ament_package() diff --git a/README.md b/README.md index 21acbbc7..369c15c5 100644 --- a/README.md +++ b/README.md @@ -27,3 +27,25 @@ doi = {10.21105/joss.00456}, URL = {http://www.theoj.org/joss-papers/joss.00456/10.21105.joss.00456.pdf} } ``` + + +## Code Formatting + +This repository uses `pre-commit` tool for code formatting. +The tool checks formatting each time you commit to a repository. +To install it locally use: + ``` + pip3 install pre-commit # (prepend `sudo` if you want to install it system wide) + ``` + +To run it initially over the whole repo you can use: + ``` + pre-commit run -a + ``` + +If you get error that something is missing on your computer, do the following for: + + - `clang-format-10` + ``` + sudo apt install clang-format-10 + ``` diff --git a/control_filters.xml b/control_filters.xml new file mode 100644 index 00000000..ca9f535b --- /dev/null +++ b/control_filters.xml @@ -0,0 +1,13 @@ + + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + + + diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp new file mode 100644 index 00000000..7e4d7fcd --- /dev/null +++ b/include/control_filters/gravity_compensation.hpp @@ -0,0 +1,222 @@ +// Copyright (c) 2021, 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_FILTERS__GRAVITY_COMPENSATION_HPP +#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/LinearMath/Transform.h" +#include "filters/filter_base.hpp" + +namespace control_filters +{ + template + class GravityCompensation : public filters::FilterBase + { + public: + /** \brief Constructor */ + GravityCompensation(); + + /** \brief Destructor */ + ~GravityCompensation(); + + /** @brief Configure filter parameters */ + virtual bool configure() override; + + /** \brief Update the filter and return the data seperately + * \param data_in T array with length width + * \param data_out T array with length width + */ + virtual bool update(const T& data_in, T& data_out) override; + + /** \brief Get most recent parameters */ + bool updateParameters(); + + private: + /** \brief Dynamic parameter callback activated when parameters change */ + // void parameterCallback(); + + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + // Storage for Calibration Values + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) + double force_z_; // Gravitational Force + + // Frames for Transformation of Gravity / CoG Vector + std::string world_frame_; + std::string sensor_frame_; + std::string force_frame_; + + // tf2 objects + std::unique_ptr p_tf_Buffer_; + std::unique_ptr p_tf_Listener_; + geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; + + bool configured_; + + uint num_transform_errors_; + }; + + template + GravityCompensation::GravityCompensation() + : logger_(rclcpp::get_logger("GravityCompensation")) + , configured_(false) + , num_transform_errors_(0) + { + } + + template + GravityCompensation::~GravityCompensation() + { + } + + template + bool GravityCompensation::configure() + { + clock_ = std::make_shared(RCL_SYSTEM_TIME); + p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); + p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); + + if(!updateParameters()){ + return false; + } + else{ + configured_ = true; + } + RCLCPP_INFO(logger_, + "Gravity Compensation Params: world frame: %s; sensor frame: %s; force frame: %s; CoG x:%f; " + "CoG y:%f; CoG z:%f; force: %f.", + world_frame_.c_str(), sensor_frame_.c_str(), force_frame_.c_str(), cog_.vector.x, cog_.vector.y, cog_.vector.z, force_z_); + + return true; + } + + template + bool GravityCompensation::update(const T& data_in, T& data_out) + { + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + //if (params_updated_) + //{ + // updateParameters(); + //} + + try + { + transform_ = p_tf_Buffer_->lookupTransform(world_frame_, data_in.header.frame_id, rclcpp::Time()); + transform_back_ = p_tf_Buffer_->lookupTransform(data_in.header.frame_id, world_frame_, rclcpp::Time()); + transform_cog_ = p_tf_Buffer_->lookupTransform(world_frame_, force_frame_, rclcpp::Time()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", ex.what()); + } + + geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; + + temp_vector_in.vector = data_in.wrench.force; + tf2::doTransform(temp_vector_in, temp_force_transformed, transform_); + + temp_vector_in.vector = data_in.wrench.torque; + tf2::doTransform(temp_vector_in, temp_torque_transformed, transform_); + + // Transform CoG Vector + geometry_msgs::msg::Vector3Stamped cog_transformed; + tf2::doTransform(cog_, cog_transformed, transform_cog_); + + // Compensate for gravity force + temp_force_transformed.vector.z += force_z_; + // Compensation Values for torque result from Crossprod of cog Vector and (0 0 G) + temp_torque_transformed.vector.x += (force_z_ * cog_transformed.vector.y); + temp_torque_transformed.vector.y -= (force_z_ * cog_transformed.vector.x); + + // Copy Message and Compensate values for Gravity Force and Resulting Torque + // geometry_msgs::WrenchStamped compensated_wrench; + data_out = data_in; + + tf2::doTransform(temp_force_transformed, temp_vector_out, transform_back_); + data_out.wrench.force = temp_vector_out.vector; + + tf2::doTransform(temp_torque_transformed, temp_vector_out, transform_back_); + data_out.wrench.torque = temp_vector_out.vector; + + return true; + } + + template + bool GravityCompensation::updateParameters() + { + //params_updated_ = false; + + if (!filters::FilterBase::getParam("world_frame", world_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param world_frame_"); + return false; + } + if (!filters::FilterBase::getParam("sensor_frame", sensor_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param sensor_frame"); + return false; + } + if (!filters::FilterBase::getParam("force_frame", force_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param force_frame"); + return false; + } + if (!filters::FilterBase::getParam("CoG_x", cog_.vector.x)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_x"); + return false; + } + if (!filters::FilterBase::getParam("CoG_y", cog_.vector.y)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_y"); + return false; + } + if (!filters::FilterBase::getParam("CoG_z", cog_.vector.z)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_z"); + return false; + } + if (!filters::FilterBase::getParam("force", force_z_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param force"); + return false; + } + return true; + } + + //template + //void GravityCompensation::parameterCallback() + //{ + // params_updated_ = true; + //} + +} // namespace iirob_filters +#endif diff --git a/package.xml b/package.xml index 2ee37d08..8f9f4665 100644 --- a/package.xml +++ b/package.xml @@ -22,9 +22,14 @@ control_msgs + filters + geometry_msgs + pluginlib rclcpp rcutils realtime_tools + tf2_geometry_msgs + ament_cmake_gmock ament_cmake_gtest diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp new file mode 100644 index 00000000..6c38439f --- /dev/null +++ b/src/control_filters/gravity_compensation.cpp @@ -0,0 +1,20 @@ +// Copyright (c) 2021, 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. + +#include "control_filters/gravity_compensation.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::GravityCompensation, + filters::FilterBase) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp new file mode 100644 index 00000000..ae9d5f05 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2021, 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. + +#include "test_gravity_compensation.hpp" + +TEST_F(GravityCompensationTest, TestGravityCompensation) +{ + node_->declare_parameter("world_frame", "world"); + node_->declare_parameter("sensor_frame", "sensor"); + node_->declare_parameter("CoG_x", 0.0); + node_->declare_parameter("CoG_y", 0.0); + node_->declare_parameter("CoG_z", 0.0); + node_->declare_parameter("force", 50.0); + + ASSERT_TRUE(gravity_compensation_.configure()); + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + ASSERT_TRUE(gravity_compensation_.update(in, out)); + + ASSERT_EQ(out.wrench.force.x, 1.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_EQ(out.wrench.force.z, 50.0); + + ASSERT_EQ(out.wrench.torque.x, 10.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); +} diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp new file mode 100644 index 00000000..a614ebb6 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2021, 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. + +#include "gmock/gmock.h" +#include + +#include "control_filters/gravity_compensation.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_gravity_compensation"); +} // namespace + +// TODO(destogl): do this +// subclassing and friending so we can access member variables + + +class GravityCompensationTest : public ::testing::Test +{ +public: + void SetUp() override + { + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + GravityCompensationTest() + : node_(std::make_shared("test_gravity_compensation")) + , executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + control_filters::GravityCompensation gravity_compensation_; + std::thread executor_thread_; +}; diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp new file mode 100644 index 00000000..22f66c1e --- /dev/null +++ b/test/control_filters/test_load_gravity_compensation.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, 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. + +// #include +// +// #include +// +// #include "controller_manager/controller_manager.hpp" +// #include "hardware_interface/resource_manager.hpp" +// #include "rclcpp/executor.hpp" +// #include "rclcpp/executors/single_threaded_executor.hpp" +// #include "rclcpp/utilities.hpp" +// #include "ros2_control_test_assets/descriptions.hpp" +// +// TEST(TestLoadAdmittanceController, load_controller) +// { +// rclcpp::init(0, nullptr); +// +// std::shared_ptr executor = +// std::make_shared(); +// +// controller_manager::ControllerManager cm(std::make_unique( +// ros2_control_test_assets::minimal_robot_urdf), +// executor, "test_controller_manager"); +// +// ASSERT_NO_THROW( +// cm.load_controller( +// "test_admittance_controller", +// "admittance_controller/AdmittanceController")); +// } From c41eb9a557b33b62cb623b96344548d787cb982a Mon Sep 17 00:00:00 2001 From: dzumkeller Date: Mon, 6 Sep 2021 19:03:29 +0200 Subject: [PATCH 02/13] Ported low_pass_filter from iirob_filters: - using the dynamic parametrization as implemented in existing gravity_compensation filter - added testcases for loading parameters --- CMakeLists.txt | 19 ++ control_filters.xml | 31 ++- include/control_filters/low_pass_filter.hpp | 217 ++++++++++++++++++ src/control_filters/low_pass_filter.cpp | 21 ++ test/control_filters/test_low_pass_filter.cpp | 91 ++++++++ test/control_filters/test_low_pass_filter.hpp | 58 +++++ 6 files changed, 426 insertions(+), 11 deletions(-) create mode 100644 include/control_filters/low_pass_filter.hpp create mode 100644 src/control_filters/low_pass_filter.cpp create mode 100644 test/control_filters/test_low_pass_filter.cpp create mode 100644 test/control_filters/test_low_pass_filter.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 942d4b05..24e78a93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) @@ -36,6 +37,7 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROL_TOOLBOX_BUILDING_LIB ament_target_dependencies(${PROJECT_NAME} control_msgs rclcpp + rclcpp_lifecycle rcutils realtime_tools) @@ -48,6 +50,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(Eigen3 REQUIRED) set(CONTROL_FILTERS_INCLUDE_DEPENDS filters @@ -67,6 +70,16 @@ target_include_directories(gravity_compensation PUBLIC ) ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) +add_library(low_pass_filter SHARED + src/control_filters/low_pass_filter.cpp +) +target_include_directories(low_pass_filter PUBLIC + $ + $ + $ +) +ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + ########## # Testing @@ -94,6 +107,11 @@ if(BUILD_TESTING) ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) target_link_libraries(test_gravity_compensation gravity_compensation) ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) + target_link_libraries(test_low_pass_filter low_pass_filter) + ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + endif() @@ -127,6 +145,7 @@ ament_export_include_directories( ament_export_libraries( ${PROJECT_NAME} gravity_compensation + low_pass_filter ) ament_package() diff --git a/control_filters.xml b/control_filters.xml index ca9f535b..15123e17 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,13 +1,22 @@ - - - - This is a gravity compensation filter working with geometry_msgs::WrenchStamped. - It subtracts the influence of a force caused by the gravitational force from force - measurements. - - + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + - + + + + This is a low pass filter working with geometry_msgs::WrenchStamped. + + + + diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp new file mode 100644 index 00000000..e02ee80f --- /dev/null +++ b/include/control_filters/low_pass_filter.hpp @@ -0,0 +1,217 @@ +// Copyright (c) 2021, 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_FILTERS__LOW_PASS_FILTER_HPP +#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP + +#include +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "filters/filter_base.hpp" +#include "filters/filter_chain.hpp" +#include +#include "rclcpp/node.hpp" + + +namespace control_filters +{ +template +class LowPassFilter : public filters::FilterBase +{ +public: + LowPassFilter(); + + ~LowPassFilter(); + + /** @brief Configure filter parameters */ + virtual bool configure(); + + bool configure(const std::string& ns); + + virtual bool update(const T& data_in, T& data_out); + + /** \brief Get most recent parameters */ + bool updateParameters(); + +private: + /** \brief Dynamic parameter callback activated when parameters change */ + void parameterCallback(); + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // Parameters + std::string parameter_namespace_; + double sampling_frequency_; + double damping_frequency_; + double damping_intensity_; + int divider_; + + // Filter parameters + double b1; + double a1; + double filtered_value, filtered_old_value, old_value; + Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + + // dynamic parameters + bool params_updated_; + bool initialized_; + bool configured_; +}; + +template +LowPassFilter::LowPassFilter() + : logger_(rclcpp::get_logger("LowPassFilter")), params_updated_(false), initialized_(false), configured_(false) +{ +} + +template +LowPassFilter::~LowPassFilter() +{ +} + +template +bool LowPassFilter::configure() +{ + configure(""); + return true; +} + +template +bool LowPassFilter::configure(const std::string& ns) +{ + if (!initialized_) + { + RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); + return false; + } + + parameter_namespace_ = ns; + + if (!updateParameters()) + { + return false; + } + + configured_ = true; + + RCLCPP_INFO(logger_, + "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " + "divider :%d", + sampling_frequency_, damping_frequency_, damping_intensity_, divider_); + + // Initialize storage Vectors + filtered_value = filtered_old_value = old_value = 0; + for (unsigned int i = 0; i < 6; i++) + { + msg_filtered(i) = msg_filtered_old(i) = msg_old(i) = 0; + } + + return true; +} + +template <> +inline bool LowPassFilter::update(const geometry_msgs::msg::WrenchStamped& data_in, + geometry_msgs::msg::WrenchStamped& data_out) +{ + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + updateParameters(); + } + + // IIR Filter + msg_filtered = b1 * msg_old + a1 * msg_filtered_old; + msg_filtered_old = msg_filtered; + + // TODO 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]; + return true; +} + +template +bool LowPassFilter::update(const T& data_in, T& data_out) +{ + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + if (!updateParameters()) + { + RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); + return false; + } + } + + data_out = b1 * old_value + a1 * filtered_old_value; + filtered_old_value = data_out; + old_value = data_in; + + return true; +} + +template +bool LowPassFilter::updateParameters() +{ + + if (!filters::FilterBase::getParam(parameter_namespace_ + "sampling_frequency", sampling_frequency_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_frequency", damping_frequency_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_intensity", damping_intensity_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "divider", divider_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); + return false; + } + + params_updated_ = false; + a1 = exp(-1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / (pow(10.0, damping_intensity_ / -10.0))); + b1 = 1.0 - a1; + return true; +} + +template +void LowPassFilter::parameterCallback() +{ + params_updated_ = true; +} + +} // namespace iirob_filters +#endif diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp new file mode 100644 index 00000000..56142344 --- /dev/null +++ b/src/control_filters/low_pass_filter.cpp @@ -0,0 +1,21 @@ +// Copyright (c) 2021, 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. + +#include "control_filters/low_pass_filter.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, + filters::FilterBase) + diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp new file mode 100644 index 00000000..5b6c517e --- /dev/null +++ b/test/control_filters/test_low_pass_filter.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2021, 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. + +#include "test_low_pass_filter.hpp" + +TEST_F(LowPassFilterTest, TestLowPassFilter) +{ + double sampling_freq = 1000.0; + double damping_freq = 20.5; + double damping_intensity = 1.25; + int divider = 3; + + bool a1, b1; + a1 = exp(-1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0))); + b1 = 1.0 - a1; + + geometry_msgs::msg::WrenchStamped in, calculated, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + + //not yet configured, should deny update + ASSERT_FALSE(low_pass_filter_.update(in,out)); + + //should deny configuration as parameters are not yet set + ASSERT_FALSE(low_pass_filter_.configure()); + + //declare parameters and configure + node_->declare_parameter("sampling_frequency", sampling_freq); + node_->declare_parameter("damping_frequency", damping_freq); + node_->declare_parameter("damping_intensity", damping_intensity); + node_->declare_parameter("divider", divider); + ASSERT_TRUE(low_pass_filter_.configure()); + + + //first filter pass, output should be zero as no old wrench was stored + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, 0.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_EQ(out.wrench.force.z, 0.0); + ASSERT_EQ(out.wrench.torque.x, 0.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); + + //second filter pass with same values: + //calculate wrench by hand + calculated.wrench.force.x = b1 * in.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z; + //check equality with low-pass-filter + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); + + //update once again and check results + //calculate wrench by hand + calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * calculated.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * calculated.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * calculated.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * calculated.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * calculated.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * calculated.wrench.torque.z; + //check equality with low-pass-filter + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); + +} diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp new file mode 100644 index 00000000..124616fa --- /dev/null +++ b/test/control_filters/test_low_pass_filter.hpp @@ -0,0 +1,58 @@ + + +// Copyright (c) 2021, 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. + +#include "gmock/gmock.h" +#include + +#include "control_filters/low_pass_filter.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_low_pass_filter"); +} // namespace + + +class LowPassFilterTest : public ::testing::Test +{ +public: + void SetUp() override + { + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + LowPassFilterTest() + : node_(std::make_shared("test_low_pass_filter")) + , executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + control_filters::LowPassFilter low_pass_filter_; + std::thread executor_thread_; +}; From ee8a6ff5542a398465f9ac25519e958cf4f689a1 Mon Sep 17 00:00:00 2001 From: Daniel Zumkeller <84941552+dzumkeller@users.noreply.github.com> Date: Thu, 30 Sep 2021 18:18:45 +0200 Subject: [PATCH 03/13] Update low_pass_filter.hpp removed parameter_namespace and added override flag --- include/control_filters/low_pass_filter.hpp | 30 +++++++-------------- 1 file changed, 9 insertions(+), 21 deletions(-) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index e02ee80f..5ad04519 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -16,10 +16,11 @@ #define CONTROL_FILTERS__LOW_PASS_FILTER_HPP #include +#include + #include "geometry_msgs/msg/wrench_stamped.hpp" #include "filters/filter_base.hpp" #include "filters/filter_chain.hpp" -#include #include "rclcpp/node.hpp" @@ -31,14 +32,11 @@ class LowPassFilter : public filters::FilterBase public: LowPassFilter(); - ~LowPassFilter(); + ~LowPassFilter() override; - /** @brief Configure filter parameters */ - virtual bool configure(); + bool configure() override; - bool configure(const std::string& ns); - - virtual bool update(const T& data_in, T& data_out); + bool update(const T& data_in, T& data_out) override; /** \brief Get most recent parameters */ bool updateParameters(); @@ -50,7 +48,6 @@ class LowPassFilter : public filters::FilterBase rclcpp::Logger logger_; // Parameters - std::string parameter_namespace_; double sampling_frequency_; double damping_frequency_; double damping_intensity_; @@ -81,21 +78,12 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() -{ - configure(""); - return true; -} - -template -bool LowPassFilter::configure(const std::string& ns) { if (!initialized_) { RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); return false; } - - parameter_namespace_ = ns; if (!updateParameters()) { @@ -184,19 +172,19 @@ template bool LowPassFilter::updateParameters() { - if (!filters::FilterBase::getParam(parameter_namespace_ + "sampling_frequency", sampling_frequency_)) { + if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_frequency", damping_frequency_)) { + if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_intensity", damping_intensity_)) { + if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "divider", divider_)) { + if (!filters::FilterBase::getParam("divider", divider_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); return false; } From f0ec3bead8d7859959a7507f49f7aa49e4c03ab9 Mon Sep 17 00:00:00 2001 From: Daniel Zumkeller <84941552+dzumkeller@users.noreply.github.com> Date: Thu, 30 Sep 2021 18:31:27 +0200 Subject: [PATCH 04/13] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- CMakeLists.txt | 2 -- control_filters.xml | 20 ++++++++++---------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 24e78a93..18a4f105 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) @@ -37,7 +36,6 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROL_TOOLBOX_BUILDING_LIB ament_target_dependencies(${PROJECT_NAME} control_msgs rclcpp - rclcpp_lifecycle rcutils realtime_tools) diff --git a/control_filters.xml b/control_filters.xml index 15123e17..f7af28dd 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,14 +1,14 @@ - - - - This is a gravity compensation filter working with geometry_msgs::WrenchStamped. - It subtracts the influence of a force caused by the gravitational force from force - measurements. - - + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + Date: Thu, 7 Oct 2021 17:31:54 +0200 Subject: [PATCH 05/13] added new testcase after review --- .../test_load_low_pass_filter.cpp | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 test/control_filters/test_load_low_pass_filter.cpp diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp new file mode 100644 index 00000000..47ac7bad --- /dev/null +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -0,0 +1,43 @@ +Copyright (c) 2021, 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. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadLowPassFilter, load_low_pass_filter) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_low_pass_filter", "controlFilters/LowPassFilter")); + + rclcpp::shutdown(); +} + + From 79aeeb2f618f06020123f5b7aec3b8661b30257f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 12 Jun 2021 17:05:10 +0200 Subject: [PATCH 06/13] Added initial files of control filters --- CMakeLists.txt | 60 ++++- README.md | 22 ++ control_filters.xml | 13 + .../control_filters/gravity_compensation.hpp | 222 ++++++++++++++++++ package.xml | 5 + src/control_filters/gravity_compensation.cpp | 20 ++ .../test_gravity_compensation.cpp | 42 ++++ .../test_gravity_compensation.hpp | 59 +++++ .../test_load_gravity_compensation.cpp | 41 ++++ 9 files changed, 481 insertions(+), 3 deletions(-) create mode 100644 control_filters.xml create mode 100644 include/control_filters/gravity_compensation.hpp create mode 100644 src/control_filters/gravity_compensation.cpp create mode 100644 test/control_filters/test_gravity_compensation.cpp create mode 100644 test/control_filters/test_gravity_compensation.hpp create mode 100644 test/control_filters/test_load_gravity_compensation.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 44f4ed5e..af596f22 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,38 @@ ament_target_dependencies(${PROJECT_NAME} rcutils realtime_tools) +######################## +# Build control filters +######################## +find_package(filters REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +set(CONTROL_FILTERS_INCLUDE_DEPENDS + filters + geometry_msgs + pluginlib + rclcpp + tf2_geometry_msgs + tf2_ros +) + +add_library(gravity_compensation SHARED + src/control_filters/gravity_compensation.cpp +) +target_include_directories(gravity_compensation PUBLIC + $ + $ +) +ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + +########## +# Testing +########## if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) @@ -53,6 +85,12 @@ if(BUILD_TESTING) ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) target_link_libraries(pid_publisher_tests ${PROJECT_NAME}) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) + + ## Control Filters + ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) + target_link_libraries(test_gravity_compensation gravity_compensation) + ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + endif() # Install @@ -64,11 +102,27 @@ install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +install(TARGETS gravity_compensation + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install pluginlib xml +pluginlib_export_plugin_description_file(filters control_filters.xml) + ament_export_dependencies( rclcpp rcutils - realtime_tools) -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) + realtime_tools + ${CONTROL_FILTERS_INCLUDE_DEPENDS} +) +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} + gravity_compensation +) ament_package() diff --git a/README.md b/README.md index 21acbbc7..369c15c5 100644 --- a/README.md +++ b/README.md @@ -27,3 +27,25 @@ doi = {10.21105/joss.00456}, URL = {http://www.theoj.org/joss-papers/joss.00456/10.21105.joss.00456.pdf} } ``` + + +## Code Formatting + +This repository uses `pre-commit` tool for code formatting. +The tool checks formatting each time you commit to a repository. +To install it locally use: + ``` + pip3 install pre-commit # (prepend `sudo` if you want to install it system wide) + ``` + +To run it initially over the whole repo you can use: + ``` + pre-commit run -a + ``` + +If you get error that something is missing on your computer, do the following for: + + - `clang-format-10` + ``` + sudo apt install clang-format-10 + ``` diff --git a/control_filters.xml b/control_filters.xml new file mode 100644 index 00000000..ca9f535b --- /dev/null +++ b/control_filters.xml @@ -0,0 +1,13 @@ + + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + + + diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp new file mode 100644 index 00000000..7e4d7fcd --- /dev/null +++ b/include/control_filters/gravity_compensation.hpp @@ -0,0 +1,222 @@ +// Copyright (c) 2021, 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_FILTERS__GRAVITY_COMPENSATION_HPP +#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/LinearMath/Transform.h" +#include "filters/filter_base.hpp" + +namespace control_filters +{ + template + class GravityCompensation : public filters::FilterBase + { + public: + /** \brief Constructor */ + GravityCompensation(); + + /** \brief Destructor */ + ~GravityCompensation(); + + /** @brief Configure filter parameters */ + virtual bool configure() override; + + /** \brief Update the filter and return the data seperately + * \param data_in T array with length width + * \param data_out T array with length width + */ + virtual bool update(const T& data_in, T& data_out) override; + + /** \brief Get most recent parameters */ + bool updateParameters(); + + private: + /** \brief Dynamic parameter callback activated when parameters change */ + // void parameterCallback(); + + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + // Storage for Calibration Values + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) + double force_z_; // Gravitational Force + + // Frames for Transformation of Gravity / CoG Vector + std::string world_frame_; + std::string sensor_frame_; + std::string force_frame_; + + // tf2 objects + std::unique_ptr p_tf_Buffer_; + std::unique_ptr p_tf_Listener_; + geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; + + bool configured_; + + uint num_transform_errors_; + }; + + template + GravityCompensation::GravityCompensation() + : logger_(rclcpp::get_logger("GravityCompensation")) + , configured_(false) + , num_transform_errors_(0) + { + } + + template + GravityCompensation::~GravityCompensation() + { + } + + template + bool GravityCompensation::configure() + { + clock_ = std::make_shared(RCL_SYSTEM_TIME); + p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); + p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); + + if(!updateParameters()){ + return false; + } + else{ + configured_ = true; + } + RCLCPP_INFO(logger_, + "Gravity Compensation Params: world frame: %s; sensor frame: %s; force frame: %s; CoG x:%f; " + "CoG y:%f; CoG z:%f; force: %f.", + world_frame_.c_str(), sensor_frame_.c_str(), force_frame_.c_str(), cog_.vector.x, cog_.vector.y, cog_.vector.z, force_z_); + + return true; + } + + template + bool GravityCompensation::update(const T& data_in, T& data_out) + { + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + //if (params_updated_) + //{ + // updateParameters(); + //} + + try + { + transform_ = p_tf_Buffer_->lookupTransform(world_frame_, data_in.header.frame_id, rclcpp::Time()); + transform_back_ = p_tf_Buffer_->lookupTransform(data_in.header.frame_id, world_frame_, rclcpp::Time()); + transform_cog_ = p_tf_Buffer_->lookupTransform(world_frame_, force_frame_, rclcpp::Time()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", ex.what()); + } + + geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; + + temp_vector_in.vector = data_in.wrench.force; + tf2::doTransform(temp_vector_in, temp_force_transformed, transform_); + + temp_vector_in.vector = data_in.wrench.torque; + tf2::doTransform(temp_vector_in, temp_torque_transformed, transform_); + + // Transform CoG Vector + geometry_msgs::msg::Vector3Stamped cog_transformed; + tf2::doTransform(cog_, cog_transformed, transform_cog_); + + // Compensate for gravity force + temp_force_transformed.vector.z += force_z_; + // Compensation Values for torque result from Crossprod of cog Vector and (0 0 G) + temp_torque_transformed.vector.x += (force_z_ * cog_transformed.vector.y); + temp_torque_transformed.vector.y -= (force_z_ * cog_transformed.vector.x); + + // Copy Message and Compensate values for Gravity Force and Resulting Torque + // geometry_msgs::WrenchStamped compensated_wrench; + data_out = data_in; + + tf2::doTransform(temp_force_transformed, temp_vector_out, transform_back_); + data_out.wrench.force = temp_vector_out.vector; + + tf2::doTransform(temp_torque_transformed, temp_vector_out, transform_back_); + data_out.wrench.torque = temp_vector_out.vector; + + return true; + } + + template + bool GravityCompensation::updateParameters() + { + //params_updated_ = false; + + if (!filters::FilterBase::getParam("world_frame", world_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param world_frame_"); + return false; + } + if (!filters::FilterBase::getParam("sensor_frame", sensor_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param sensor_frame"); + return false; + } + if (!filters::FilterBase::getParam("force_frame", force_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param force_frame"); + return false; + } + if (!filters::FilterBase::getParam("CoG_x", cog_.vector.x)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_x"); + return false; + } + if (!filters::FilterBase::getParam("CoG_y", cog_.vector.y)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_y"); + return false; + } + if (!filters::FilterBase::getParam("CoG_z", cog_.vector.z)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_z"); + return false; + } + if (!filters::FilterBase::getParam("force", force_z_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param force"); + return false; + } + return true; + } + + //template + //void GravityCompensation::parameterCallback() + //{ + // params_updated_ = true; + //} + +} // namespace iirob_filters +#endif diff --git a/package.xml b/package.xml index 8d8b13a7..90cf11aa 100644 --- a/package.xml +++ b/package.xml @@ -22,9 +22,14 @@ control_msgs + filters + geometry_msgs + pluginlib rclcpp rcutils realtime_tools + tf2_geometry_msgs + ament_cmake_gmock ament_cmake_gtest diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp new file mode 100644 index 00000000..6c38439f --- /dev/null +++ b/src/control_filters/gravity_compensation.cpp @@ -0,0 +1,20 @@ +// Copyright (c) 2021, 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. + +#include "control_filters/gravity_compensation.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::GravityCompensation, + filters::FilterBase) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp new file mode 100644 index 00000000..ae9d5f05 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2021, 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. + +#include "test_gravity_compensation.hpp" + +TEST_F(GravityCompensationTest, TestGravityCompensation) +{ + node_->declare_parameter("world_frame", "world"); + node_->declare_parameter("sensor_frame", "sensor"); + node_->declare_parameter("CoG_x", 0.0); + node_->declare_parameter("CoG_y", 0.0); + node_->declare_parameter("CoG_z", 0.0); + node_->declare_parameter("force", 50.0); + + ASSERT_TRUE(gravity_compensation_.configure()); + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + ASSERT_TRUE(gravity_compensation_.update(in, out)); + + ASSERT_EQ(out.wrench.force.x, 1.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_EQ(out.wrench.force.z, 50.0); + + ASSERT_EQ(out.wrench.torque.x, 10.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); +} diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp new file mode 100644 index 00000000..a614ebb6 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2021, 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. + +#include "gmock/gmock.h" +#include + +#include "control_filters/gravity_compensation.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_gravity_compensation"); +} // namespace + +// TODO(destogl): do this +// subclassing and friending so we can access member variables + + +class GravityCompensationTest : public ::testing::Test +{ +public: + void SetUp() override + { + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + GravityCompensationTest() + : node_(std::make_shared("test_gravity_compensation")) + , executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + control_filters::GravityCompensation gravity_compensation_; + std::thread executor_thread_; +}; diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp new file mode 100644 index 00000000..22f66c1e --- /dev/null +++ b/test/control_filters/test_load_gravity_compensation.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, 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. + +// #include +// +// #include +// +// #include "controller_manager/controller_manager.hpp" +// #include "hardware_interface/resource_manager.hpp" +// #include "rclcpp/executor.hpp" +// #include "rclcpp/executors/single_threaded_executor.hpp" +// #include "rclcpp/utilities.hpp" +// #include "ros2_control_test_assets/descriptions.hpp" +// +// TEST(TestLoadAdmittanceController, load_controller) +// { +// rclcpp::init(0, nullptr); +// +// std::shared_ptr executor = +// std::make_shared(); +// +// controller_manager::ControllerManager cm(std::make_unique( +// ros2_control_test_assets::minimal_robot_urdf), +// executor, "test_controller_manager"); +// +// ASSERT_NO_THROW( +// cm.load_controller( +// "test_admittance_controller", +// "admittance_controller/AdmittanceController")); +// } From 1ea8b37e27a05e7976d9fc568dd16f24e1c81506 Mon Sep 17 00:00:00 2001 From: dzumkeller Date: Mon, 6 Sep 2021 19:03:29 +0200 Subject: [PATCH 07/13] Ported low_pass_filter from iirob_filters: - using the dynamic parametrization as implemented in existing gravity_compensation filter - added testcases for loading parameters --- CMakeLists.txt | 19 ++ control_filters.xml | 31 ++- include/control_filters/low_pass_filter.hpp | 217 ++++++++++++++++++ src/control_filters/low_pass_filter.cpp | 21 ++ test/control_filters/test_low_pass_filter.cpp | 91 ++++++++ test/control_filters/test_low_pass_filter.hpp | 58 +++++ 6 files changed, 426 insertions(+), 11 deletions(-) create mode 100644 include/control_filters/low_pass_filter.hpp create mode 100644 src/control_filters/low_pass_filter.cpp create mode 100644 test/control_filters/test_low_pass_filter.cpp create mode 100644 test/control_filters/test_low_pass_filter.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index af596f22..12396911 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) @@ -36,6 +37,7 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROL_TOOLBOX_BUILDING_LIB ament_target_dependencies(${PROJECT_NAME} control_msgs rclcpp + rclcpp_lifecycle rcutils realtime_tools) @@ -48,6 +50,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(Eigen3 REQUIRED) set(CONTROL_FILTERS_INCLUDE_DEPENDS filters @@ -67,6 +70,16 @@ target_include_directories(gravity_compensation PUBLIC ) ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) +add_library(low_pass_filter SHARED + src/control_filters/low_pass_filter.cpp +) +target_include_directories(low_pass_filter PUBLIC + $ + $ + $ +) +ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + ########## # Testing @@ -90,6 +103,11 @@ if(BUILD_TESTING) ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) target_link_libraries(test_gravity_compensation gravity_compensation) ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) + target_link_libraries(test_low_pass_filter low_pass_filter) + ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + endif() @@ -123,6 +141,7 @@ ament_export_include_directories( ament_export_libraries( ${PROJECT_NAME} gravity_compensation + low_pass_filter ) ament_package() diff --git a/control_filters.xml b/control_filters.xml index ca9f535b..15123e17 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,13 +1,22 @@ - - - - This is a gravity compensation filter working with geometry_msgs::WrenchStamped. - It subtracts the influence of a force caused by the gravitational force from force - measurements. - - + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + - + + + + This is a low pass filter working with geometry_msgs::WrenchStamped. + + + + diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp new file mode 100644 index 00000000..e02ee80f --- /dev/null +++ b/include/control_filters/low_pass_filter.hpp @@ -0,0 +1,217 @@ +// Copyright (c) 2021, 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_FILTERS__LOW_PASS_FILTER_HPP +#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP + +#include +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "filters/filter_base.hpp" +#include "filters/filter_chain.hpp" +#include +#include "rclcpp/node.hpp" + + +namespace control_filters +{ +template +class LowPassFilter : public filters::FilterBase +{ +public: + LowPassFilter(); + + ~LowPassFilter(); + + /** @brief Configure filter parameters */ + virtual bool configure(); + + bool configure(const std::string& ns); + + virtual bool update(const T& data_in, T& data_out); + + /** \brief Get most recent parameters */ + bool updateParameters(); + +private: + /** \brief Dynamic parameter callback activated when parameters change */ + void parameterCallback(); + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // Parameters + std::string parameter_namespace_; + double sampling_frequency_; + double damping_frequency_; + double damping_intensity_; + int divider_; + + // Filter parameters + double b1; + double a1; + double filtered_value, filtered_old_value, old_value; + Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + + // dynamic parameters + bool params_updated_; + bool initialized_; + bool configured_; +}; + +template +LowPassFilter::LowPassFilter() + : logger_(rclcpp::get_logger("LowPassFilter")), params_updated_(false), initialized_(false), configured_(false) +{ +} + +template +LowPassFilter::~LowPassFilter() +{ +} + +template +bool LowPassFilter::configure() +{ + configure(""); + return true; +} + +template +bool LowPassFilter::configure(const std::string& ns) +{ + if (!initialized_) + { + RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); + return false; + } + + parameter_namespace_ = ns; + + if (!updateParameters()) + { + return false; + } + + configured_ = true; + + RCLCPP_INFO(logger_, + "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " + "divider :%d", + sampling_frequency_, damping_frequency_, damping_intensity_, divider_); + + // Initialize storage Vectors + filtered_value = filtered_old_value = old_value = 0; + for (unsigned int i = 0; i < 6; i++) + { + msg_filtered(i) = msg_filtered_old(i) = msg_old(i) = 0; + } + + return true; +} + +template <> +inline bool LowPassFilter::update(const geometry_msgs::msg::WrenchStamped& data_in, + geometry_msgs::msg::WrenchStamped& data_out) +{ + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + updateParameters(); + } + + // IIR Filter + msg_filtered = b1 * msg_old + a1 * msg_filtered_old; + msg_filtered_old = msg_filtered; + + // TODO 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]; + return true; +} + +template +bool LowPassFilter::update(const T& data_in, T& data_out) +{ + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + if (!updateParameters()) + { + RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); + return false; + } + } + + data_out = b1 * old_value + a1 * filtered_old_value; + filtered_old_value = data_out; + old_value = data_in; + + return true; +} + +template +bool LowPassFilter::updateParameters() +{ + + if (!filters::FilterBase::getParam(parameter_namespace_ + "sampling_frequency", sampling_frequency_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_frequency", damping_frequency_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_intensity", damping_intensity_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); + return false; + } + if (!filters::FilterBase::getParam(parameter_namespace_ + "divider", divider_)) { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); + return false; + } + + params_updated_ = false; + a1 = exp(-1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / (pow(10.0, damping_intensity_ / -10.0))); + b1 = 1.0 - a1; + return true; +} + +template +void LowPassFilter::parameterCallback() +{ + params_updated_ = true; +} + +} // namespace iirob_filters +#endif diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp new file mode 100644 index 00000000..56142344 --- /dev/null +++ b/src/control_filters/low_pass_filter.cpp @@ -0,0 +1,21 @@ +// Copyright (c) 2021, 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. + +#include "control_filters/low_pass_filter.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, + filters::FilterBase) + diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp new file mode 100644 index 00000000..5b6c517e --- /dev/null +++ b/test/control_filters/test_low_pass_filter.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2021, 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. + +#include "test_low_pass_filter.hpp" + +TEST_F(LowPassFilterTest, TestLowPassFilter) +{ + double sampling_freq = 1000.0; + double damping_freq = 20.5; + double damping_intensity = 1.25; + int divider = 3; + + bool a1, b1; + a1 = exp(-1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0))); + b1 = 1.0 - a1; + + geometry_msgs::msg::WrenchStamped in, calculated, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + + //not yet configured, should deny update + ASSERT_FALSE(low_pass_filter_.update(in,out)); + + //should deny configuration as parameters are not yet set + ASSERT_FALSE(low_pass_filter_.configure()); + + //declare parameters and configure + node_->declare_parameter("sampling_frequency", sampling_freq); + node_->declare_parameter("damping_frequency", damping_freq); + node_->declare_parameter("damping_intensity", damping_intensity); + node_->declare_parameter("divider", divider); + ASSERT_TRUE(low_pass_filter_.configure()); + + + //first filter pass, output should be zero as no old wrench was stored + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, 0.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_EQ(out.wrench.force.z, 0.0); + ASSERT_EQ(out.wrench.torque.x, 0.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); + + //second filter pass with same values: + //calculate wrench by hand + calculated.wrench.force.x = b1 * in.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z; + //check equality with low-pass-filter + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); + + //update once again and check results + //calculate wrench by hand + calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * calculated.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * calculated.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * calculated.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * calculated.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * calculated.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * calculated.wrench.torque.z; + //check equality with low-pass-filter + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); + +} diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp new file mode 100644 index 00000000..124616fa --- /dev/null +++ b/test/control_filters/test_low_pass_filter.hpp @@ -0,0 +1,58 @@ + + +// Copyright (c) 2021, 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. + +#include "gmock/gmock.h" +#include + +#include "control_filters/low_pass_filter.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_low_pass_filter"); +} // namespace + + +class LowPassFilterTest : public ::testing::Test +{ +public: + void SetUp() override + { + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + LowPassFilterTest() + : node_(std::make_shared("test_low_pass_filter")) + , executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + control_filters::LowPassFilter low_pass_filter_; + std::thread executor_thread_; +}; From 06391d8dc430bccd018304e3c124db5853d8dd97 Mon Sep 17 00:00:00 2001 From: dzumkeller Date: Thu, 7 Oct 2021 17:31:54 +0200 Subject: [PATCH 08/13] added new testcase after review --- .../test_load_low_pass_filter.cpp | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 test/control_filters/test_load_low_pass_filter.cpp diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp new file mode 100644 index 00000000..47ac7bad --- /dev/null +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -0,0 +1,43 @@ +Copyright (c) 2021, 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. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadLowPassFilter, load_low_pass_filter) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_low_pass_filter", "controlFilters/LowPassFilter")); + + rclcpp::shutdown(); +} + + From 1cebf05cb20d45a1ead410db7622c182ea3a2cc9 Mon Sep 17 00:00:00 2001 From: Daniel Zumkeller <84941552+dzumkeller@users.noreply.github.com> Date: Thu, 30 Sep 2021 18:18:45 +0200 Subject: [PATCH 09/13] Update low_pass_filter.hpp removed parameter_namespace and added override flag --- include/control_filters/low_pass_filter.hpp | 30 +++++++-------------- 1 file changed, 9 insertions(+), 21 deletions(-) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index e02ee80f..5ad04519 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -16,10 +16,11 @@ #define CONTROL_FILTERS__LOW_PASS_FILTER_HPP #include +#include + #include "geometry_msgs/msg/wrench_stamped.hpp" #include "filters/filter_base.hpp" #include "filters/filter_chain.hpp" -#include #include "rclcpp/node.hpp" @@ -31,14 +32,11 @@ class LowPassFilter : public filters::FilterBase public: LowPassFilter(); - ~LowPassFilter(); + ~LowPassFilter() override; - /** @brief Configure filter parameters */ - virtual bool configure(); + bool configure() override; - bool configure(const std::string& ns); - - virtual bool update(const T& data_in, T& data_out); + bool update(const T& data_in, T& data_out) override; /** \brief Get most recent parameters */ bool updateParameters(); @@ -50,7 +48,6 @@ class LowPassFilter : public filters::FilterBase rclcpp::Logger logger_; // Parameters - std::string parameter_namespace_; double sampling_frequency_; double damping_frequency_; double damping_intensity_; @@ -81,21 +78,12 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() -{ - configure(""); - return true; -} - -template -bool LowPassFilter::configure(const std::string& ns) { if (!initialized_) { RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); return false; } - - parameter_namespace_ = ns; if (!updateParameters()) { @@ -184,19 +172,19 @@ template bool LowPassFilter::updateParameters() { - if (!filters::FilterBase::getParam(parameter_namespace_ + "sampling_frequency", sampling_frequency_)) { + if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_frequency", damping_frequency_)) { + if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "damping_intensity", damping_intensity_)) { + if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); return false; } - if (!filters::FilterBase::getParam(parameter_namespace_ + "divider", divider_)) { + if (!filters::FilterBase::getParam("divider", divider_)) { RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); return false; } From c0f365a1bfe2342dee20752803a2af63cf41a0b0 Mon Sep 17 00:00:00 2001 From: Daniel Zumkeller <84941552+dzumkeller@users.noreply.github.com> Date: Thu, 30 Sep 2021 18:31:27 +0200 Subject: [PATCH 10/13] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- CMakeLists.txt | 2 -- control_filters.xml | 20 ++++++++++---------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 12396911..94cb13d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) @@ -37,7 +36,6 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROL_TOOLBOX_BUILDING_LIB ament_target_dependencies(${PROJECT_NAME} control_msgs rclcpp - rclcpp_lifecycle rcutils realtime_tools) diff --git a/control_filters.xml b/control_filters.xml index 15123e17..f7af28dd 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,14 +1,14 @@ - - - - This is a gravity compensation filter working with geometry_msgs::WrenchStamped. - It subtracts the influence of a force caused by the gravitational force from force - measurements. - - + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + Date: Fri, 29 Oct 2021 17:44:21 +0200 Subject: [PATCH 11/13] Update version of gravity compensation to use parameter_handler and optimize code. --- CMakeLists.txt | 23 +- .../control_filters/gravity_compensation.hpp | 302 ++++++++---------- src/control_filters/gravity_compensation.cpp | 65 ++++ 3 files changed, 203 insertions(+), 187 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 94cb13d9..7a0be99f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,17 +40,17 @@ ament_target_dependencies(${PROJECT_NAME} realtime_tools) ######################## -# Build control filters +# Control filters ######################## find_package(filters REQUIRED) find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(Eigen3 REQUIRED) set(CONTROL_FILTERS_INCLUDE_DEPENDS + Eigen3 filters geometry_msgs pluginlib @@ -62,20 +62,15 @@ set(CONTROL_FILTERS_INCLUDE_DEPENDS add_library(gravity_compensation SHARED src/control_filters/gravity_compensation.cpp ) -target_include_directories(gravity_compensation PUBLIC - $ - $ -) +target_include_directories(gravity_compensation PUBLIC include) +target_link_libraries(gravity_compensation parameter_handler) ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) add_library(low_pass_filter SHARED src/control_filters/low_pass_filter.cpp ) -target_include_directories(low_pass_filter PUBLIC - $ - $ - $ -) +target_include_directories(low_pass_filter PUBLIC include) +target_link_libraries(low_pass_filter parameter_handler) ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) @@ -101,12 +96,10 @@ if(BUILD_TESTING) ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) target_link_libraries(test_gravity_compensation gravity_compensation) ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) - + ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) target_link_libraries(test_low_pass_filter low_pass_filter) ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) - - endif() # Install @@ -118,7 +111,7 @@ install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib RUNTIME DESTINATION bin) -install(TARGETS gravity_compensation +install(TARGETS gravity_compensation low_pass_filter ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 7e4d7fcd..9320280a 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -15,208 +15,166 @@ #ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP #define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "geometry_msgs/msg/vector3_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2_ros/transform_listener.h" -#include "tf2/LinearMath/Transform.h" +#include "control_toolbox/parameter_handler.hpp" #include "filters/filter_base.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include +#include "tf2_ros/transform_listener.h" namespace control_filters { - template - class GravityCompensation : public filters::FilterBase - { - public: - /** \brief Constructor */ - GravityCompensation(); - - /** \brief Destructor */ - ~GravityCompensation(); - - /** @brief Configure filter parameters */ - virtual bool configure() override; - - /** \brief Update the filter and return the data seperately - * \param data_in T array with length width - * \param data_out T array with length width - */ - virtual bool update(const T& data_in, T& data_out) override; - - /** \brief Get most recent parameters */ - bool updateParameters(); - - private: - /** \brief Dynamic parameter callback activated when parameters change */ - // void parameterCallback(); - - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - // Storage for Calibration Values - geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) - double force_z_; // Gravitational Force - - // Frames for Transformation of Gravity / CoG Vector - std::string world_frame_; - std::string sensor_frame_; - std::string force_frame_; - - // tf2 objects - std::unique_ptr p_tf_Buffer_; - std::unique_ptr p_tf_Listener_; - geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; - - bool configured_; - - uint num_transform_errors_; - }; - - template - GravityCompensation::GravityCompensation() - : logger_(rclcpp::get_logger("GravityCompensation")) - , configured_(false) - , num_transform_errors_(0) +class GravityCompensationParameters : public control_toolbox::ParameterHandler +{ +public: + GravityCompensationParameters(const std::string & params_prefix) : + control_toolbox::ParameterHandler (params_prefix, 0, 4, 3) { + add_string_parameter("world_frame", false); + add_string_parameter("sensor_frame", false); + add_string_parameter("force_frame", false); + + add_double_parameter("CoG.x", true); + add_double_parameter("CoG.y", true); + add_double_parameter("CoG.z", true); + add_double_parameter("force", true); } - template - GravityCompensation::~GravityCompensation() + bool check_if_parameters_are_valid() override { - } + bool ret = true; - template - bool GravityCompensation::configure() - { - clock_ = std::make_shared(RCL_SYSTEM_TIME); - p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); - p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); + // Check if any string parameter is empty + ret = !empty_parameter_in_list(string_parameters_); - if(!updateParameters()){ - return false; - } - else{ - configured_ = true; + for (size_t i = 0; i < 4; ++i) + { + if (std::isnan(double_parameters_[i].second)) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), + "Parameter '%s' has to be set", double_parameters_[i].first.name.c_str()); + ret = false; + } } - RCLCPP_INFO(logger_, - "Gravity Compensation Params: world frame: %s; sensor frame: %s; force frame: %s; CoG x:%f; " - "CoG y:%f; CoG z:%f; force: %f.", - world_frame_.c_str(), sensor_frame_.c_str(), force_frame_.c_str(), cog_.vector.x, cog_.vector.y, cog_.vector.z, force_z_); - return true; + return ret; } - template - bool GravityCompensation::update(const T& data_in, T& data_out) + void update_storage() override { - if (!configured_) - { - RCLCPP_ERROR(logger_, "Filter is not configured"); - return false; - } + world_frame_ = string_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "World frame: %s", world_frame_.c_str()); + sensor_frame_ = string_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Sensor frame: %s", sensor_frame_.c_str()); + force_frame_ = string_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Force frame: %s", force_frame_.c_str()); + + cog_.vector.x = double_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "CoG X is %e", cog_.vector.x); + cog_.vector.y = double_parameters_[1].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "CoG Y is %e", cog_.vector.y); + cog_.vector.z = double_parameters_[2].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "CoG Z is %e", cog_.vector.z); + + force_z_ = double_parameters_[3].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Force is %e", force_z_); + } - //if (params_updated_) - //{ - // updateParameters(); - //} + // Frames for Transformation of Gravity / CoG Vector + std::string world_frame_; + std::string sensor_frame_; + std::string force_frame_; - try - { - transform_ = p_tf_Buffer_->lookupTransform(world_frame_, data_in.header.frame_id, rclcpp::Time()); - transform_back_ = p_tf_Buffer_->lookupTransform(data_in.header.frame_id, world_frame_, rclcpp::Time()); - transform_cog_ = p_tf_Buffer_->lookupTransform(world_frame_, force_frame_, rclcpp::Time()); - } - catch (const tf2::TransformException& ex) - { - RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", ex.what()); - } + // Storage for Calibration Values + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) + double force_z_; // Gravitational Force +}; - geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; +template +class GravityCompensation : public filters::FilterBase +{ +public: + /** \brief Constructor */ + GravityCompensation(); - temp_vector_in.vector = data_in.wrench.force; - tf2::doTransform(temp_vector_in, temp_force_transformed, transform_); + /** \brief Destructor */ + ~GravityCompensation(); - temp_vector_in.vector = data_in.wrench.torque; - tf2::doTransform(temp_vector_in, temp_torque_transformed, transform_); + /** @brief Configure filter parameters */ + virtual bool configure() override; - // Transform CoG Vector - geometry_msgs::msg::Vector3Stamped cog_transformed; - tf2::doTransform(cog_, cog_transformed, transform_cog_); + /** \brief Update the filter and return the data seperately + * \param data_in T array with length width + * \param data_out T array with length width + */ + virtual bool update(const T& data_in, T& data_out) override; - // Compensate for gravity force - temp_force_transformed.vector.z += force_z_; - // Compensation Values for torque result from Crossprod of cog Vector and (0 0 G) - temp_torque_transformed.vector.x += (force_z_ * cog_transformed.vector.y); - temp_torque_transformed.vector.y -= (force_z_ * cog_transformed.vector.x); +private: + rclcpp::Clock::SharedPtr clock_; - // Copy Message and Compensate values for Gravity Force and Resulting Torque - // geometry_msgs::WrenchStamped compensated_wrench; - data_out = data_in; + std::shared_ptr logger_; + std::unique_ptr parameters_; - tf2::doTransform(temp_force_transformed, temp_vector_out, transform_back_); - data_out.wrench.force = temp_vector_out.vector; + // tf2 objects + std::unique_ptr p_tf_Buffer_; + std::unique_ptr p_tf_Listener_; + geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; - tf2::doTransform(temp_torque_transformed, temp_vector_out, transform_back_); - data_out.wrench.torque = temp_vector_out.vector; + // Callback for updating dynamic parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; +}; - return true; - } +template +GravityCompensation::GravityCompensation() +{ +} - template - bool GravityCompensation::updateParameters() - { - //params_updated_ = false; +template +GravityCompensation::~GravityCompensation() +{ +} - if (!filters::FilterBase::getParam("world_frame", world_frame_)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param world_frame_"); - return false; - } - if (!filters::FilterBase::getParam("sensor_frame", sensor_frame_)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param sensor_frame"); - return false; - } - if (!filters::FilterBase::getParam("force_frame", force_frame_)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param force_frame"); - return false; - } - if (!filters::FilterBase::getParam("CoG_x", cog_.vector.x)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param CoG_x"); - return false; - } - if (!filters::FilterBase::getParam("CoG_y", cog_.vector.y)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param CoG_y"); - return false; - } - if (!filters::FilterBase::getParam("CoG_z", cog_.vector.z)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param CoG_z"); - return false; - } - if (!filters::FilterBase::getParam("force", force_z_)) { - RCLCPP_ERROR( - this->logging_interface_->get_logger(), - "Gravitiy Compensator did not find param force"); - return false; - } - return true; +template +bool GravityCompensation::configure() +{ + clock_ = std::make_shared(RCL_SYSTEM_TIME); + p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); + p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); + + // TODO(destogl): get here filter + logger_.reset(new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); + parameters_.reset(new GravityCompensationParameters(this->param_prefix_)); + + parameters_->initialize(this->params_interface_, logger_->get_name()); + + parameters_->declare_parameters(); + + if(!parameters_->get_parameters()) + { + return false; } - //template - //void GravityCompensation::parameterCallback() - //{ - // params_updated_ = true; - //} + // Add callback to dynamically update parameters + on_set_callback_handle_ = this->params_interface_->add_on_set_parameters_callback( + [this](const std::vector & parameters) { + + return parameters_->set_parameter_callback(parameters); + }); + + return true; +} } // namespace iirob_filters #endif diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 6c38439f..38b1fed6 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -14,6 +14,71 @@ #include "control_filters/gravity_compensation.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" + +namespace control_filters +{ +template <> +bool GravityCompensation::update( + const geometry_msgs::msg::WrenchStamped& data_in, geometry_msgs::msg::WrenchStamped& data_out) +{ + if (!this->configured_) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + return false; + } + + parameters_->update(); + + try + { + transform_ = p_tf_Buffer_->lookupTransform( + parameters_->world_frame_, data_in.header.frame_id, rclcpp::Time()); + transform_back_ = p_tf_Buffer_->lookupTransform( + data_in.header.frame_id, parameters_->world_frame_, rclcpp::Time()); + transform_cog_ = p_tf_Buffer_->lookupTransform( + parameters_->world_frame_, parameters_->force_frame_, rclcpp::Time()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 5000, "%s", ex.what()); + } + + geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; + + // TODO(destogl): change this when `doTransform` for wrenches is merged into geometry2 + temp_vector_in.vector = data_in.wrench.force; + tf2::doTransform(temp_vector_in, temp_force_transformed, transform_); + + temp_vector_in.vector = data_in.wrench.torque; + tf2::doTransform(temp_vector_in, temp_torque_transformed, transform_); + + // Transform CoG Vector + geometry_msgs::msg::Vector3Stamped cog_transformed; + tf2::doTransform(parameters_->cog_, cog_transformed, transform_cog_); + + // Compensate for gravity force + temp_force_transformed.vector.z += parameters_->force_z_; + // Compensation Values for torque result from cross-product of cog Vector and (0 0 G) + temp_torque_transformed.vector.x += (parameters_->force_z_ * cog_transformed.vector.y); + temp_torque_transformed.vector.y -= (parameters_->force_z_ * cog_transformed.vector.x); + + // Copy Message and Compensate values for Gravity Force and Resulting Torque + data_out = data_in; + + // TODO(destogl): change this when `doTransform` for wrenches is merged into geometry2 + tf2::doTransform(temp_force_transformed, temp_vector_out, transform_back_); + data_out.wrench.force = temp_vector_out.vector; + + tf2::doTransform(temp_torque_transformed, temp_vector_out, transform_back_); + data_out.wrench.torque = temp_vector_out.vector; + + return true; +} + +} // namespace control_filters + #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(control_filters::GravityCompensation, From 9457c9f921be69c61303e25bcf751dbdce5ca4f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 29 Oct 2021 18:10:58 +0200 Subject: [PATCH 12/13] Apply formatting to filters. --- .../control_filters/gravity_compensation.hpp | 59 ++-- include/control_filters/low_pass_filter.hpp | 275 +++++++++--------- src/control_filters/gravity_compensation.cpp | 16 +- src/control_filters/low_pass_filter.cpp | 6 +- 4 files changed, 180 insertions(+), 176 deletions(-) diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 9320280a..bb3815eb 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -12,13 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP -#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP +#ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ +#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ + +#include +#include +#include #include "control_toolbox/parameter_handler.hpp" #include "filters/filter_base.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include +#include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" namespace control_filters @@ -26,8 +30,8 @@ namespace control_filters class GravityCompensationParameters : public control_toolbox::ParameterHandler { public: - GravityCompensationParameters(const std::string & params_prefix) : - control_toolbox::ParameterHandler (params_prefix, 0, 4, 3) + explicit GravityCompensationParameters(const std::string & params_prefix) + : control_toolbox::ParameterHandler(params_prefix, 0, 4, 3) { add_string_parameter("world_frame", false); add_string_parameter("sensor_frame", false); @@ -51,8 +55,8 @@ class GravityCompensationParameters : public control_toolbox::ParameterHandler if (std::isnan(double_parameters_[i].second)) { RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), - "Parameter '%s' has to be set", double_parameters_[i].first.name.c_str()); + logger_name_.c_str(), "Parameter '%s' has to be set", + double_parameters_[i].first.name.c_str()); ret = false; } } @@ -63,30 +67,18 @@ class GravityCompensationParameters : public control_toolbox::ParameterHandler void update_storage() override { world_frame_ = string_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "World frame: %s", world_frame_.c_str()); + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "World frame: %s", world_frame_.c_str()); sensor_frame_ = string_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Sensor frame: %s", sensor_frame_.c_str()); + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Sensor frame: %s", sensor_frame_.c_str()); force_frame_ = string_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Force frame: %s", force_frame_.c_str()); + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Force frame: %s", force_frame_.c_str()); cog_.vector.x = double_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "CoG X is %e", cog_.vector.x); + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG X is %e", cog_.vector.x); cog_.vector.y = double_parameters_[1].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "CoG Y is %e", cog_.vector.y); + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG Y is %e", cog_.vector.y); cog_.vector.z = double_parameters_[2].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "CoG Z is %e", cog_.vector.z); + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG Z is %e", cog_.vector.z); force_z_ = double_parameters_[3].second; RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Force is %e", force_z_); @@ -113,13 +105,13 @@ class GravityCompensation : public filters::FilterBase ~GravityCompensation(); /** @brief Configure filter parameters */ - virtual bool configure() override; + bool configure() override; - /** \brief Update the filter and return the data seperately + /** \brief Update the filter and return the data separately * \param data_in T array with length width * \param data_out T array with length width */ - virtual bool update(const T& data_in, T& data_out) override; + bool update(const T & data_in, T & data_out) override; private: rclcpp::Clock::SharedPtr clock_; @@ -154,14 +146,15 @@ bool GravityCompensation::configure() p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); // TODO(destogl): get here filter - logger_.reset(new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); + logger_.reset( + new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); parameters_.reset(new GravityCompensationParameters(this->param_prefix_)); parameters_->initialize(this->params_interface_, logger_->get_name()); parameters_->declare_parameters(); - if(!parameters_->get_parameters()) + if (!parameters_->get_parameters()) { return false; } @@ -169,12 +162,12 @@ bool GravityCompensation::configure() // Add callback to dynamically update parameters on_set_callback_handle_ = this->params_interface_->add_on_set_parameters_callback( [this](const std::vector & parameters) { - return parameters_->set_parameter_callback(parameters); }); return true; } -} // namespace iirob_filters -#endif +} // namespace control_filters + +#endif // CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 5ad04519..c7c19895 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -12,62 +12,64 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP -#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP +#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ -#include #include +#include -#include "geometry_msgs/msg/wrench_stamped.hpp" #include "filters/filter_base.hpp" #include "filters/filter_chain.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/node.hpp" - namespace control_filters { template class LowPassFilter : public filters::FilterBase { public: - LowPassFilter(); + LowPassFilter(); - ~LowPassFilter() override; + ~LowPassFilter() override; - bool configure() override; + bool configure() override; - bool update(const T& data_in, T& data_out) override; + bool update(const T & data_in, T & data_out) override; - /** \brief Get most recent parameters */ - bool updateParameters(); + /** \brief Get most recent parameters */ + bool updateParameters(); private: - /** \brief Dynamic parameter callback activated when parameters change */ - void parameterCallback(); - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; - - // Parameters - double sampling_frequency_; - double damping_frequency_; - double damping_intensity_; - int divider_; - - // Filter parameters - double b1; - double a1; - double filtered_value, filtered_old_value, old_value; - Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; - - // dynamic parameters - bool params_updated_; - bool initialized_; - bool configured_; + /** \brief Dynamic parameter callback activated when parameters change */ + void parameterCallback(); + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // Parameters + double sampling_frequency_; + double damping_frequency_; + double damping_intensity_; + int divider_; + + // Filter parameters + double b1; + double a1; + double filtered_value, filtered_old_value, old_value; + Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + + // dynamic parameters + bool params_updated_; + bool initialized_; + bool configured_; }; template LowPassFilter::LowPassFilter() - : logger_(rclcpp::get_logger("LowPassFilter")), params_updated_(false), initialized_(false), configured_(false) +: logger_(rclcpp::get_logger("LowPassFilter")), + params_updated_(false), + initialized_(false), + configured_(false) { } @@ -79,127 +81,134 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - if (!initialized_) - { - RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); - return false; - } - - if (!updateParameters()) - { - return false; - } - - configured_ = true; - - RCLCPP_INFO(logger_, - "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " - "divider :%d", - sampling_frequency_, damping_frequency_, damping_intensity_, divider_); - - // Initialize storage Vectors - filtered_value = filtered_old_value = old_value = 0; - for (unsigned int i = 0; i < 6; i++) - { - msg_filtered(i) = msg_filtered_old(i) = msg_old(i) = 0; - } - - return true; + if (!initialized_) + { + RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); + return false; + } + + if (!updateParameters()) + { + return false; + } + + configured_ = true; + + RCLCPP_INFO( + logger_, + "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " + "divider :%d", + sampling_frequency_, damping_frequency_, damping_intensity_, divider_); + + // Initialize storage Vectors + filtered_value = filtered_old_value = old_value = 0; + for (unsigned int i = 0; i < 6; i++) + { + msg_filtered(i) = msg_filtered_old(i) = msg_old(i) = 0; + } + + return true; } template <> -inline bool LowPassFilter::update(const geometry_msgs::msg::WrenchStamped& data_in, - geometry_msgs::msg::WrenchStamped& data_out) +inline bool LowPassFilter::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!configured_) - { - RCLCPP_ERROR(logger_, "Filter is not configured"); - return false; - } - - if (params_updated_) - { - updateParameters(); - } - - // IIR Filter - msg_filtered = b1 * msg_old + a1 * msg_filtered_old; - msg_filtered_old = msg_filtered; - - // TODO 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]; - return true; + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + updateParameters(); + } + + // 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]; + return true; } template -bool LowPassFilter::update(const T& data_in, T& data_out) +bool LowPassFilter::update(const T & data_in, T & data_out) { - if (!configured_) - { - RCLCPP_ERROR(logger_, "Filter is not configured"); - return false; - } - - if (params_updated_) + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + if (params_updated_) + { + if (!updateParameters()) { - if (!updateParameters()) - { - RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); - return false; - } + RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); + return false; } + } - data_out = b1 * old_value + a1 * filtered_old_value; - filtered_old_value = data_out; - old_value = data_in; + data_out = b1 * old_value + a1 * filtered_old_value; + filtered_old_value = data_out; + old_value = data_in; - return true; + return true; } template bool LowPassFilter::updateParameters() { - - if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); - return false; - } - if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); - return false; - } - if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); - return false; - } - if (!filters::FilterBase::getParam("divider", divider_)) { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); - return false; - } - - params_updated_ = false; - a1 = exp(-1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / (pow(10.0, damping_intensity_ / -10.0))); - b1 = 1.0 - a1; - return true; + if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) + { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); + return false; + } + if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) + { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); + return false; + } + if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) + { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); + return false; + } + if (!filters::FilterBase::getParam("divider", divider_)) + { + RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); + return false; + } + + params_updated_ = false; + a1 = exp( + -1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / + (pow(10.0, damping_intensity_ / -10.0))); + b1 = 1.0 - a1; + return true; } template void LowPassFilter::parameterCallback() { - params_updated_ = true; + params_updated_ = true; } -} // namespace iirob_filters -#endif +} // namespace control_filters + +#endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 38b1fed6..14d0383a 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -14,14 +14,14 @@ #include "control_filters/gravity_compensation.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" #include "geometry_msgs/msg/vector3_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" namespace control_filters { template <> bool GravityCompensation::update( - const geometry_msgs::msg::WrenchStamped& data_in, geometry_msgs::msg::WrenchStamped& data_out) + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { if (!this->configured_) { @@ -38,14 +38,15 @@ bool GravityCompensation::update( transform_back_ = p_tf_Buffer_->lookupTransform( data_in.header.frame_id, parameters_->world_frame_, rclcpp::Time()); transform_cog_ = p_tf_Buffer_->lookupTransform( - parameters_->world_frame_, parameters_->force_frame_, rclcpp::Time()); + parameters_->world_frame_, parameters_->force_frame_, rclcpp::Time()); } - catch (const tf2::TransformException& ex) + catch (const tf2::TransformException & ex) { RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 5000, "%s", ex.what()); } - geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; + geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, + temp_vector_in, temp_vector_out; // TODO(destogl): change this when `doTransform` for wrenches is merged into geometry2 temp_vector_in.vector = data_in.wrench.force; @@ -81,5 +82,6 @@ bool GravityCompensation::update( #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(control_filters::GravityCompensation, - filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + control_filters::GravityCompensation, + filters::FilterBase) diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp index 56142344..3aba0735 100644 --- a/src/control_filters/low_pass_filter.cpp +++ b/src/control_filters/low_pass_filter.cpp @@ -16,6 +16,6 @@ #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, - filters::FilterBase) - +PLUGINLIB_EXPORT_CLASS( + control_filters::LowPassFilter, + filters::FilterBase) From f2d02af0215c77f4bb0f245ba536ee59f215e0f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 30 Oct 2021 12:11:50 +0200 Subject: [PATCH 13/13] Update filters to use full impl of parameter hanlder. --- control_filters.xml | 25 ++- .../control_filters/gravity_compensation.hpp | 14 +- include/control_filters/low_pass_filter.hpp | 208 +++++++++--------- src/control_filters/gravity_compensation.cpp | 1 + src/control_filters/low_pass_filter.cpp | 2 + .../test_gravity_compensation.hpp | 14 +- .../test_load_low_pass_filter.cpp | 44 ++-- test/control_filters/test_low_pass_filter.hpp | 16 +- 8 files changed, 184 insertions(+), 140 deletions(-) diff --git a/control_filters.xml b/control_filters.xml index f7af28dd..46fc8688 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,7 +1,7 @@ - This is a gravity compensation filter working with geometry_msgs::WrenchStamped. @@ -11,12 +11,19 @@ - - - This is a low pass filter working with geometry_msgs::WrenchStamped. - - + + + This is a low pass filter working with a double value. + + + + + This is a low pass filter working with geometry_msgs::WrenchStamped. + + diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index bb3815eb..0292599e 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -21,7 +21,7 @@ #include "control_toolbox/parameter_handler.hpp" #include "filters/filter_base.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -31,7 +31,7 @@ class GravityCompensationParameters : public control_toolbox::ParameterHandler { public: explicit GravityCompensationParameters(const std::string & params_prefix) - : control_toolbox::ParameterHandler(params_prefix, 0, 4, 3) + : control_toolbox::ParameterHandler(params_prefix, 0, 0, 4, 3) { add_string_parameter("world_frame", false); add_string_parameter("sensor_frame", false); @@ -115,17 +115,16 @@ class GravityCompensation : public filters::FilterBase private: rclcpp::Clock::SharedPtr clock_; - std::shared_ptr logger_; std::unique_ptr parameters_; - // tf2 objects + // Callback for updating dynamic parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; + + // Filter objects std::unique_ptr p_tf_Buffer_; std::unique_ptr p_tf_Listener_; geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; - - // Callback for updating dynamic parameters - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; }; template @@ -145,7 +144,6 @@ bool GravityCompensation::configure() p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); - // TODO(destogl): get here filter logger_.reset( new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); parameters_.reset(new GravityCompensationParameters(this->param_prefix_)); diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index c7c19895..cf99fcac 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -17,14 +17,88 @@ #include #include +#include +#include +#include +#include "control_toolbox/parameter_handler.hpp" #include "filters/filter_base.hpp" -#include "filters/filter_chain.hpp" + #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "rclcpp/node.hpp" namespace control_filters { +class LowPassParameters : public control_toolbox::ParameterHandler +{ +public: + explicit LowPassParameters(const std::string & params_prefix) + : control_toolbox::ParameterHandler(params_prefix, 0, 1, 3) + { + add_double_parameter("sampling_frequency", false); + add_double_parameter("damping_frequency", false); + add_double_parameter("damping_intensity", false); + + add_integer_parameter("divider", false); + } + + bool check_if_parameters_are_valid() override + { + bool ret = true; + + // Check if any string parameter is empty + ret = !empty_parameter_in_list(string_parameters_); + + for (size_t i = 0; i < 3; ++i) + { + if (std::isnan(double_parameters_[i].second)) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Parameter '%s' has to be set", + double_parameters_[i].first.name.c_str()); + ret = false; + } + } + + if (integer_parameters_[0].second < 0) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Parameter '%s' has to be positive", + integer_parameters_[0].first.name.c_str()); + } + + return ret; + } + + void update_storage() override + { + sampling_frequency_ = double_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Sampling frequency is %e", sampling_frequency_); + damping_frequency_ = double_parameters_[1].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Damping frequency is %e", damping_frequency_); + damping_intensity_ = double_parameters_[2].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Damping intensity is %e", damping_intensity_); + + divider_ = integer_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Divider %d", divider_); + + a1_ = exp( + -1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / + (pow(10.0, damping_intensity_ / -10.0))); + b1_ = 1.0 - a1_; + } + + // Parameters from parameter server + double sampling_frequency_; + double damping_frequency_; + double damping_intensity_; + + int divider_; + + // Filter Parameters + double a1_; + double b1_; +}; + template class LowPassFilter : public filters::FilterBase { @@ -37,39 +111,22 @@ class LowPassFilter : public filters::FilterBase bool update(const T & data_in, T & data_out) override; - /** \brief Get most recent parameters */ - bool updateParameters(); - private: - /** \brief Dynamic parameter callback activated when parameters change */ - void parameterCallback(); - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr logger_; + std::unique_ptr parameters_; - // Parameters - double sampling_frequency_; - double damping_frequency_; - double damping_intensity_; - int divider_; + // Callback for updating dynamic parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; // Filter parameters - double b1; - double a1; + // TODO(destogl): we should do this more intelligently using only one set of types double filtered_value, filtered_old_value, old_value; Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; - - // dynamic parameters - bool params_updated_; - bool initialized_; - bool configured_; }; template LowPassFilter::LowPassFilter() -: logger_(rclcpp::get_logger("LowPassFilter")), - params_updated_(false), - initialized_(false), - configured_(false) { } @@ -81,32 +138,34 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - if (!initialized_) - { - RCLCPP_INFO(logger_, "Node is not initialized... call setNode()"); - return false; - } + clock_ = std::make_shared(RCL_SYSTEM_TIME); + logger_.reset( + new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); + parameters_.reset(new LowPassParameters(this->param_prefix_)); + + parameters_->initialize(this->params_interface_, logger_->get_name()); - if (!updateParameters()) + parameters_->declare_parameters(); + + if (!parameters_->get_parameters()) { return false; } - configured_ = true; - - RCLCPP_INFO( - logger_, - "Low Pass Filter Params: sampling frequency: %f; damping frequency: %f; damping intensity :%f; " - "divider :%d", - sampling_frequency_, damping_frequency_, damping_intensity_, divider_); - // Initialize storage Vectors filtered_value = filtered_old_value = old_value = 0; - for (unsigned int i = 0; i < 6; i++) + // 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; + msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; } + // Add callback to dynamically update parameters + on_set_callback_handle_ = this->params_interface_->add_on_set_parameters_callback( + [this](const std::vector & parameters) { + return parameters_->set_parameter_callback(parameters); + }); + return true; } @@ -114,22 +173,19 @@ template <> inline bool LowPassFilter::update( const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!configured_) + if (!this->configured_) { - RCLCPP_ERROR(logger_, "Filter is not configured"); + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } - if (params_updated_) - { - updateParameters(); - } + parameters_->update(); // IIR Filter - msg_filtered = b1 * msg_old + a1 * msg_filtered_old; + msg_filtered = parameters_->b1_ * msg_old + parameters_->a1_ * msg_filtered_old; msg_filtered_old = msg_filtered; - // TODO(destogl) use wrenchMsgToEigen + // 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; @@ -149,66 +205,22 @@ inline bool LowPassFilter::update( template bool LowPassFilter::update(const T & data_in, T & data_out) { - if (!configured_) + if (!this->configured_) { - RCLCPP_ERROR(logger_, "Filter is not configured"); + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } - if (params_updated_) - { - if (!updateParameters()) - { - RCLCPP_ERROR(logger_, "Unable to update as not all parameters are set"); - return false; - } - } + parameters_->update(); - data_out = b1 * old_value + a1 * filtered_old_value; + // Filter + data_out = parameters_->b1_ * old_value + parameters_->a1_ * filtered_old_value; filtered_old_value = data_out; old_value = data_in; return true; } -template -bool LowPassFilter::updateParameters() -{ - if (!filters::FilterBase::getParam("sampling_frequency", sampling_frequency_)) - { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter sampling_frequency"); - return false; - } - if (!filters::FilterBase::getParam("damping_frequency", damping_frequency_)) - { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_frequency"); - return false; - } - if (!filters::FilterBase::getParam("damping_intensity", damping_intensity_)) - { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter damping_intensity"); - return false; - } - if (!filters::FilterBase::getParam("divider", divider_)) - { - RCLCPP_ERROR(logger_, "Low pass filter did not find parameter divider"); - return false; - } - - params_updated_ = false; - a1 = exp( - -1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / - (pow(10.0, damping_intensity_ / -10.0))); - b1 = 1.0 - a1; - return true; -} - -template -void LowPassFilter::parameterCallback() -{ - params_updated_ = true; -} - } // namespace control_filters #endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 14d0383a..ca5f41f9 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -16,6 +16,7 @@ #include "geometry_msgs/msg/vector3_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace control_filters { diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp index 3aba0735..ccd5fe2b 100644 --- a/src/control_filters/low_pass_filter.cpp +++ b/src/control_filters/low_pass_filter.cpp @@ -16,6 +16,8 @@ #include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, filters::FilterBase) + PLUGINLIB_EXPORT_CLASS( control_filters::LowPassFilter, filters::FilterBase) diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp index a614ebb6..39980a72 100644 --- a/test/control_filters/test_gravity_compensation.hpp +++ b/test/control_filters/test_gravity_compensation.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gmock/gmock.h" +#ifndef CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ +#define CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ + +#include #include +#include "gmock/gmock.h" #include "control_filters/gravity_compensation.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/rclcpp.hpp" namespace @@ -26,7 +31,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_gravity_compensati // TODO(destogl): do this // subclassing and friending so we can access member variables - class GravityCompensationTest : public ::testing::Test { public: @@ -37,8 +41,8 @@ class GravityCompensationTest : public ::testing::Test } GravityCompensationTest() - : node_(std::make_shared("test_gravity_compensation")) - , executor_(std::make_shared()) + : node_(std::make_shared("test_gravity_compensation")), + executor_(std::make_shared()) { } @@ -57,3 +61,5 @@ class GravityCompensationTest : public ::testing::Test control_filters::GravityCompensation gravity_compensation_; std::thread executor_thread_; }; + +#endif // CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp index 47ac7bad..8be57b2c 100644 --- a/test/control_filters/test_load_low_pass_filter.cpp +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -1,16 +1,16 @@ -Copyright (c) 2021, 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. +// Copyright (c) 2021, 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. #include #include @@ -34,10 +34,24 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_low_pass_filter", "controlFilters/LowPassFilter")); + ASSERT_NO_THROW(cm.load_controller("test_low_pass_filter", "controlFilters/LowPassFilter")); rclcpp::shutdown(); } +TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_low_pass_filter", "controlFilters/LowPassFilterWrench")); + rclcpp::shutdown(); +} diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp index 124616fa..c2dd4e27 100644 --- a/test/control_filters/test_low_pass_filter.hpp +++ b/test/control_filters/test_low_pass_filter.hpp @@ -1,5 +1,3 @@ - - // Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -14,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gmock/gmock.h" +#ifndef CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ + +#include #include +#include "gmock/gmock.h" #include "control_filters/low_pass_filter.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/rclcpp.hpp" namespace @@ -25,7 +28,6 @@ namespace static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_low_pass_filter"); } // namespace - class LowPassFilterTest : public ::testing::Test { public: @@ -36,8 +38,8 @@ class LowPassFilterTest : public ::testing::Test } LowPassFilterTest() - : node_(std::make_shared("test_low_pass_filter")) - , executor_(std::make_shared()) + : node_(std::make_shared("test_low_pass_filter")), + executor_(std::make_shared()) { } @@ -56,3 +58,5 @@ class LowPassFilterTest : public ::testing::Test control_filters::LowPassFilter low_pass_filter_; std::thread executor_thread_; }; + +#endif // CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_