From 5471301e74c0f05b93872179192e838ba523b30e 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/25] Added initial files of control filters --- CMakeLists.txt | 58 ++++- README.md | 22 ++ control_filters.xml | 13 + .../control_filters/gravity_compensation.hpp | 223 ++++++++++++++++++ package.xml | 6 + 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, 483 insertions(+), 1 deletion(-) 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 fda0ecb7..2ca495c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,38 @@ target_include_directories(control_toolbox PUBLIC ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") +######################## +# 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 + 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) @@ -55,19 +87,43 @@ if(BUILD_TESTING) ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) target_link_libraries(pid_publisher_tests control_toolbox) 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 install( DIRECTORY include/ DESTINATION include/control_toolbox ) install(TARGETS control_toolbox EXPORT export_control_toolbox + ARCHIVE DESTINATION lib + 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_targets(export_control_toolbox HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS}) +ament_export_include_directories( + include +) +ament_export_libraries( + control_toolbox + 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..3ef222d5 --- /dev/null +++ b/include/control_filters/gravity_compensation.hpp @@ -0,0 +1,223 @@ +// 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.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.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 52c9b467..02f6442b 100644 --- a/package.xml +++ b/package.xml @@ -19,9 +19,15 @@ ament_cmake control_msgs + filters + geometry_msgs + pluginlib rclcpp rcutils realtime_tools + tf2 + tf2_ros + 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 7532c93dd170125abb521069bbcc05e031d488fa 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 02/25] 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 --- control_filters.xml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/control_filters.xml b/control_filters.xml index ca9f535b..d99ee262 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -3,11 +3,11 @@ - - 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. + + + + From d61437ad100b6f62f99a2c86d4577b7cf8f2e4ee 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 03/25] Added initial files of control filters --- control_filters.xml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/control_filters.xml b/control_filters.xml index d99ee262..ca9f535b 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -3,11 +3,11 @@ - - 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. + + + + From e63a7a1ad748b71a36e10f0285f8bcfba77d4c02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 29 Oct 2021 17:44:21 +0200 Subject: [PATCH 04/25] Update version of gravity compensation to use parameter_handler and optimize code. --- CMakeLists.txt | 24 +- .../control_filters/gravity_compensation.hpp | 296 ++++++++---------- src/control_filters/gravity_compensation.cpp | 65 ++++ 3 files changed, 213 insertions(+), 172 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2ca495c8..988fd22c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,7 +42,24 @@ ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") ######################## -# Build control filters +# Parameter handler +######################## +add_library(parameter_handler SHARED + src/parameter_handler.cpp +) + +target_include_directories(parameter_handler PUBLIC + $ + $ +) +ament_target_dependencies(parameter_handler + rcl_interfaces + rclcpp + rcutils +) + +######################## +# Control filters ######################## find_package(filters REQUIRED) find_package(geometry_msgs REQUIRED) @@ -68,6 +85,7 @@ target_include_directories(gravity_compensation PUBLIC $ $ ) +target_link_libraries(gravity_compensation parameter_handler) ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) ########## @@ -100,7 +118,7 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox +install(TARGETS control_toolbox parameter_handler EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -115,7 +133,6 @@ install(TARGETS gravity_compensation # Install pluginlib xml pluginlib_export_plugin_description_file(filters control_filters.xml) - ament_export_targets(export_control_toolbox HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS}) ament_export_include_directories( @@ -124,6 +141,7 @@ ament_export_include_directories( ament_export_libraries( control_toolbox gravity_compensation + parameter_handler ) ament_package() diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 3ef222d5..a4ccef8b 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -15,8 +15,7 @@ #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 "control_toolbox/parameter_handler.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" @@ -25,199 +24,158 @@ 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 d5a55a746608a24a80a676aea49f88e4e873c275 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 05/25] Apply formatting to filters. --- .../control_filters/gravity_compensation.hpp | 62 ++++++++----------- src/control_filters/gravity_compensation.cpp | 16 ++--- 2 files changed, 36 insertions(+), 42 deletions(-) diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index a4ccef8b..bb3815eb 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -12,23 +12,26 @@ // 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 "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" -#include "tf2/LinearMath/Transform.h" -#include "filters/filter_base.hpp" +#include "tf2_ros/transform_listener.h" 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); @@ -52,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; } } @@ -64,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_); @@ -114,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_; @@ -155,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; } @@ -170,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/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) From 381794729da1092eef1ea456e2ea6209ae2ed04a 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 06/25] Update filters to use full impl of parameter hanlder. --- control_filters.xml | 20 +++++++++---------- .../control_filters/gravity_compensation.hpp | 14 ++++++------- src/control_filters/gravity_compensation.cpp | 1 + .../test_gravity_compensation.hpp | 14 +++++++++---- 4 files changed, 27 insertions(+), 22 deletions(-) diff --git a/control_filters.xml b/control_filters.xml index ca9f535b..0fcb8922 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,13 +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. - - - - + + 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 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/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/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_ From c0465986867fbe5391b5ae7476f188f5426351e7 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 6 Mar 2023 16:40:34 +0100 Subject: [PATCH 07/25] Fixed PR115 especially for humble fixed gravity compensation calculated values fixed guard condition issues (rclcpp init must be called before) fixed configure issues (low-level must be configured to avoid undefined logger) --- CMakeLists.txt | 1 + include/control_toolbox/parameter_handler.hpp | 387 ++++++++++++++++++ package.xml | 1 + src/parameter_handler.cpp | 216 ++++++++++ .../test_gravity_compensation.cpp | 15 +- .../test_gravity_compensation.hpp | 10 +- 6 files changed, 621 insertions(+), 9 deletions(-) create mode 100644 include/control_toolbox/parameter_handler.hpp create mode 100644 src/parameter_handler.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 988fd22c..1e54bf95 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp + rcl_interfaces rcutils realtime_tools ) diff --git a/include/control_toolbox/parameter_handler.hpp b/include/control_toolbox/parameter_handler.hpp new file mode 100644 index 00000000..f38fd093 --- /dev/null +++ b/include/control_toolbox/parameter_handler.hpp @@ -0,0 +1,387 @@ +// 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. +// +/// \author: Denis Stogl + +#ifndef CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ +#define CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rcutils/logging_macros.h" + +namespace control_toolbox +{ +using rclcpp::ParameterType; + +namespace impl +{ +inline std::string normalize_params_prefix(std::string prefix) +{ + if (!prefix.empty()) + { + if ('.' != prefix.back()) + { + prefix += '.'; + } + } + return prefix; +} +} // namespace impl + +struct Parameter +{ + Parameter() = default; + + explicit Parameter(const std::string & name, const uint8_t type, const bool configurable = false) + : name(name), type(type), configurable(configurable) + { + } + + std::string name; + uint8_t type; + bool configurable; +}; + +class ParameterHandler +{ +public: + ParameterHandler( + const std::string & params_prefix = "", int nr_bool_params = 0, int nr_integer_params = 0, + int nr_double_params = 0, int nr_string_params = 0, int nr_byte_array_params = 0, + int nr_bool_array_params = 0, int nr_integer_array_params = 0, int nr_double_array_params = 0, + int nr_string_array_params = 0); + + virtual ~ParameterHandler() = default; + + void initialize(rclcpp::Node::SharedPtr node) + { + initialize(node->get_node_parameters_interface(), node->get_logger().get_name()); + } + + void initialize( + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, + const std::string & logger_name = "::ControllerParameters") + { + params_interface_ = node_params; + logger_name_ = logger_name + "." + "parameters"; + } + + void declare_parameters(); + + /** + * Gets all defined parameters from parameter server after parameters are declared. + * Optionally, check parameters' validity and update storage. + * + * \param[in] check_validity check validity after getting parameters (default: **true**). + * \param[in] update update parameters in storage after getting them (default: **true**). + * \return true if all parameters are read successfully, false if a parameter is not provided or + * parameter configuration is wrong. + */ + bool get_parameters(bool check_validity = true, bool update = true); + + /** + * Update all storage variables from the parameter maps. + * Parameters will be only updated if they are previously read from parameter server or dynamic + * reconfigure callback occurred. + * + * \param[in] check_validity check validity before updating parameters (default: **true**). + * \return is update was successful, i.e., parameters are valid. If \p check_validity is set + * **false**, the result is always **true**. + */ + bool update(bool check_validity = true); + + rcl_interfaces::msg::SetParametersResult set_parameter_callback( + const std::vector & parameters); + +protected: + /** + * Override this method to implement custom parameters check. + * Default implementation does not checks anything, always returns true. + * + * \return **true** if parameters are valid, **false** otherwise. + */ + virtual bool check_if_parameters_are_valid() { return true; } + + /** + * Mapping between parameter storage and implementation members. + * The order of parameter in storage is the order of adding parameters to the storage. + * E.g., to access the value of 5-th string parameter added to storage use: + * `fifth_string_param_ = string_parameters_[4].second; + * where `fifth_string_param_` is the name of the member of a child class. + */ + virtual void update_storage() = 0; + +protected: + void add_bool_parameter( + const std::string & name, const bool & configurable = false, const bool & default_value = false) + { + bool_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), + default_value}); + } + + void add_integer_parameter( + const std::string & name, const bool & configurable = false, + const int & default_value = std::numeric_limits::quiet_NaN()) + { + integer_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER, configurable), + default_value}); + } + + void add_double_parameter( + const std::string & name, const bool & configurable = false, + const double & default_value = std::numeric_limits::quiet_NaN()) + { + double_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), + default_value}); + } + + void add_string_parameter( + const std::string & name, const bool & configurable = false, + const std::string & default_value = "") + { + string_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), + default_value}); + } + + void add_byte_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + byte_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BYTE_ARRAY, configurable), + default_value}); + } + + void add_bool_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + bool_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL_ARRAY, configurable), + default_value}); + } + + void add_integer_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + integer_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER_ARRAY, configurable), + default_value}); + } + void add_double_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + double_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), + default_value}); + } + + void add_string_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + string_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), + default_value}); + } + + template + bool empty_parameter_in_list(const std::vector> & parameters) + { + bool ret = false; + for (const auto & parameter : parameters) + { + if (parameter.second.empty()) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); + ret = true; + } + } + return ret; + } + + bool empty_parameter_in_list(const std::vector> & parameters) + { + return empty_numeric_parameter_in_list(parameters); + } + + bool empty_parameter_in_list(const std::vector> & parameters) + { + return empty_numeric_parameter_in_list(parameters); + } + + template + bool find_and_assign_parameter_value( + std::vector> & parameter_list, + const rclcpp::Parameter & input_parameter) + { + auto is_in_list = [&](const auto & parameter) { + return parameter.first.name == input_parameter.get_name(); + }; + + auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); + + if (it != parameter_list.end()) + { + if (!it->first.configurable) + { + throw std::runtime_error( + "Parameter " + input_parameter.get_name() + " is not configurable."); + } + else + { + it->second = input_parameter.get_value(); + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), + input_parameter.value_to_string().c_str()); + return true; + } + } + else + { + return false; + } + } + + // Storage members + std::vector> bool_parameters_; + std::vector> integer_parameters_; + std::vector> double_parameters_; + std::vector> string_parameters_; + std::vector>> byte_array_parameters_; + std::vector>> bool_array_parameters_; + std::vector>> integer_array_parameters_; + std::vector>> double_array_parameters_; + std::vector>> string_array_parameters_; + + // Functional members + bool declared_; + bool up_to_date_; + std::string params_prefix_; + + // Input/Output members to ROS 2 + std::string logger_name_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; + +private: + template + void declare_parameters_from_list( + rclcpp::Node::SharedPtr node, const std::vector> & parameters) + { + for (const auto & parameter : parameters) + { + node->declare_parameter(parameter.first.name, parameter.second); + } + } + + template + void declare_parameters_from_list(const std::vector> & parameters) + { + for (const auto & parameter : parameters) + { + // declare parameter only if it does not already exists + if (!params_interface_->has_parameter(parameter.first.name)) + { + rclcpp::ParameterValue default_parameter_value(parameter.second); + rcl_interfaces::msg::ParameterDescriptor descr; + descr.name = parameter.first.name; + descr.type = parameter.first.type; + descr.read_only = false; + + params_interface_->declare_parameter(parameter.first.name, default_parameter_value, descr); + } + } + } + + template + bool empty_numeric_parameter_in_list(const std::vector> & parameters) + { + bool ret = false; + for (const auto & parameter : parameters) + { + if (std::isnan(parameter.second)) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); + ret = true; + } + } + return ret; + } + + template + bool get_parameters_from_list( + const rclcpp::Node::SharedPtr node, std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = node->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } + } + return ret; + } + + template + bool get_parameters_from_list(std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = params_interface_->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } + } + return ret; + } +}; + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ diff --git a/package.xml b/package.xml index 02f6442b..f6a98f64 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ filters geometry_msgs pluginlib + rcl_interfaces rclcpp rcutils realtime_tools diff --git a/src/parameter_handler.cpp b/src/parameter_handler.cpp new file mode 100644 index 00000000..52ca742e --- /dev/null +++ b/src/parameter_handler.cpp @@ -0,0 +1,216 @@ +// 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. +// +/// \author: Denis Stogl + +#include "control_toolbox/parameter_handler.hpp" + +#include +#include + +#include "rcutils/logging_macros.h" + +namespace control_toolbox +{ +ParameterHandler::ParameterHandler( + const std::string & params_prefix, int nr_bool_params, int nr_integer_params, + int nr_double_params, int nr_string_params, int nr_byte_array_params, int nr_bool_array_params, + int nr_integer_array_params, int nr_double_array_params, int nr_string_array_params) +: declared_(false), up_to_date_(false), params_prefix_("") +{ + params_prefix_ = impl::normalize_params_prefix(params_prefix); + + bool_parameters_.reserve(nr_bool_params); + integer_parameters_.reserve(nr_integer_params); + double_parameters_.reserve(nr_double_params); + string_parameters_.reserve(nr_string_params); + byte_array_parameters_.reserve(nr_byte_array_params); + bool_array_parameters_.reserve(nr_bool_array_params); + integer_array_parameters_.reserve(nr_integer_array_params); + double_array_parameters_.reserve(nr_double_array_params); + string_array_parameters_.reserve(nr_string_array_params); +} + +void ParameterHandler::declare_parameters() +{ + if (!declared_) + { + declare_parameters_from_list(bool_parameters_); + declare_parameters_from_list(integer_parameters_); + declare_parameters_from_list(double_parameters_); + declare_parameters_from_list(string_parameters_); + declare_parameters_from_list(byte_array_parameters_); + declare_parameters_from_list(bool_array_parameters_); + declare_parameters_from_list(integer_array_parameters_); + declare_parameters_from_list(double_array_parameters_); + declare_parameters_from_list(string_array_parameters_); + + declared_ = true; + } + else + { + RCUTILS_LOG_WARN_NAMED( + logger_name_.c_str(), + "Parameters already declared. Declaration should be done only once. " + "Nothing bad will happen, but please correct your code-logic."); + } +} + +bool ParameterHandler::get_parameters(bool check_validity, bool update) +{ + // TODO(destogl): Should we "auto-declare" parameters here? + if (!declared_) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Can not get parameters. Please declare them first."); + return false; + } + + bool ret = false; + + // If parameters are updated using dynamic reconfigure callback then there is no need to read + // them again. To ignore multiple manual reads + ret = + get_parameters_from_list(bool_parameters_) && get_parameters_from_list(integer_parameters_) && + get_parameters_from_list(double_parameters_) && get_parameters_from_list(string_parameters_) && + get_parameters_from_list(byte_array_parameters_) && + get_parameters_from_list(bool_array_parameters_) && + get_parameters_from_list(integer_array_parameters_) && + get_parameters_from_list(double_array_parameters_) && + get_parameters_from_list(string_array_parameters_); + + if (ret && check_validity) + { + ret = this->check_if_parameters_are_valid(); + } + + // If it is all good until now the parameters are not up to date anymore + if (ret) + { + up_to_date_ = false; + } + + if (ret && update) + { + ret = this->update(false); + } + + return ret; +} + +bool ParameterHandler::update(bool check_validity) +{ + bool ret = true; + + // Let's make this efficient and execute code only if parameters are updated + if (!up_to_date_) + { + if (check_validity) + { + ret = this->check_if_parameters_are_valid(); + } + + if (ret) + { + this->update_storage(); + } + else + { + RCUTILS_LOG_WARN_NAMED( + logger_name_.c_str(), "Parameters are not valid and therefore will not be updated"); + } + // reset variable to update parameters only when this is needed + up_to_date_ = true; + } + + return ret; +} + +rcl_interfaces::msg::SetParametersResult ParameterHandler::set_parameter_callback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + // TODO(destogl): this is probably executed in another thread --> mutex protection is needed. + for (const auto & input_parameter : parameters) + { + bool found = false; + + try + { + found = find_and_assign_parameter_value(bool_parameters_, input_parameter); + if (!found) + { + found = find_and_assign_parameter_value(integer_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(double_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(string_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(byte_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(bool_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(integer_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(double_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(string_array_parameters_, input_parameter); + } + + RCUTILS_LOG_INFO_EXPRESSION_NAMED( + found, logger_name_.c_str(), + "Dynamic parameters got changed! Maybe you have to restart controller to update the " + "parameters internally."); + + if (found) + { + up_to_date_ = false; + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + result.successful = false; + result.reason = e.what(); + RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); + break; + } + catch (const std::runtime_error & e) + { + result.successful = false; + result.reason = e.what(); + RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); + break; + } + } + + return result; +} + +} // namespace control_toolbox diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index ae9d5f05..e90b53dd 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -16,21 +16,26 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) { + std::shared_ptr> filter_ = + std::make_shared>(); + 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_frame", "world"); + 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()); + ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 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_TRUE(filter_->update(in, out)); ASSERT_EQ(out.wrench.force.x, 1.0); ASSERT_EQ(out.wrench.force.y, 0.0); diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp index 39980a72..7e048e14 100644 --- a/test/control_filters/test_gravity_compensation.hpp +++ b/test/control_filters/test_gravity_compensation.hpp @@ -40,10 +40,11 @@ class GravityCompensationTest : public ::testing::Test executor_thread_ = std::thread([this]() { executor_->spin(); }); } - GravityCompensationTest() - : node_(std::make_shared("test_gravity_compensation")), - executor_(std::make_shared()) + GravityCompensationTest() { + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_gravity_compensation"); + executor_ = std::make_shared(); } void TearDown() override @@ -53,12 +54,13 @@ class GravityCompensationTest : public ::testing::Test { executor_thread_.join(); } + node_.reset(); + rclcpp::shutdown(); } protected: rclcpp::Node::SharedPtr node_; rclcpp::Executor::SharedPtr executor_; - control_filters::GravityCompensation gravity_compensation_; std::thread executor_thread_; }; From e6d3e276ac4080742a3d0ca9d7de4b79ff0c0119 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 7 Mar 2023 21:59:38 +0100 Subject: [PATCH 08/25] Switched to generate_parameter_library --- CMakeLists.txt | 33 +- control_filters.xml | 4 +- .../control_filters/gravity_compensation.hpp | 116 ++---- include/control_toolbox/parameter_handler.hpp | 387 ------------------ package.xml | 2 +- src/control_filters/gravity_compensation.cpp | 22 +- ...ravity_compensation_filter_parameters.yaml | 43 ++ src/parameter_handler.cpp | 216 ---------- .../test_gravity_compensation.cpp | 54 ++- .../test_gravity_compensation.hpp | 2 +- 10 files changed, 155 insertions(+), 724 deletions(-) delete mode 100644 include/control_toolbox/parameter_handler.hpp create mode 100644 src/control_filters/gravity_compensation_filter_parameters.yaml delete mode 100644 src/parameter_handler.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e54bf95..b0e600c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,29 +42,13 @@ target_include_directories(control_toolbox PUBLIC ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") -######################## -# Parameter handler -######################## -add_library(parameter_handler SHARED - src/parameter_handler.cpp -) - -target_include_directories(parameter_handler PUBLIC - $ - $ -) -ament_target_dependencies(parameter_handler - rcl_interfaces - rclcpp - rcutils -) - ######################## # Control filters ######################## find_package(filters REQUIRED) find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -73,6 +57,7 @@ set(CONTROL_FILTERS_INCLUDE_DEPENDS filters geometry_msgs pluginlib + generate_parameter_library rclcpp tf2_geometry_msgs tf2 @@ -86,7 +71,11 @@ target_include_directories(gravity_compensation PUBLIC $ $ ) -target_link_libraries(gravity_compensation parameter_handler) +generate_parameter_library( + gravity_compensation_filter_parameters + src/control_filters/gravity_compensation_filter_parameters.yaml +) +target_link_libraries(gravity_compensation gravity_compensation_filter_parameters) ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) ########## @@ -109,7 +98,7 @@ if(BUILD_TESTING) ## Control Filters ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) - target_link_libraries(test_gravity_compensation gravity_compensation) + target_link_libraries(test_gravity_compensation gravity_compensation gravity_compensation_filter_parameters) ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() @@ -119,13 +108,13 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox parameter_handler +install(TARGETS control_toolbox EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) -install(TARGETS gravity_compensation +install(TARGETS gravity_compensation gravity_compensation_filter_parameters ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -142,7 +131,7 @@ ament_export_include_directories( ament_export_libraries( control_toolbox gravity_compensation - parameter_handler + gravity_compensation_filter_parameters ) ament_package() diff --git a/control_filters.xml b/control_filters.xml index 0fcb8922..1c60dbc5 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,8 +1,8 @@ + type="control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>" + base_class_type="filters::FilterBase<geometry_msgs::msg::WrenchStamped>"> 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 diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 0292599e..9d749376 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -19,7 +19,7 @@ #include #include -#include "control_toolbox/parameter_handler.hpp" +#include "gravity_compensation_filter_parameters.hpp" #include "filters/filter_base.hpp" #include "geometry_msgs/msg/vector3_stamped.hpp" #include "tf2_ros/buffer.h" @@ -27,72 +27,8 @@ namespace control_filters { -class GravityCompensationParameters : public control_toolbox::ParameterHandler -{ -public: - explicit GravityCompensationParameters(const std::string & params_prefix) - : control_toolbox::ParameterHandler(params_prefix, 0, 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); - } - 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 < 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; - } - } - - return ret; - } - - 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()); - 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_); - } - // Frames for Transformation of Gravity / CoG Vector - std::string world_frame_; - std::string sensor_frame_; - std::string force_frame_; - - // Storage for Calibration Values - geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) - double force_z_; // Gravitational Force -}; template class GravityCompensation : public filters::FilterBase @@ -113,13 +49,30 @@ class GravityCompensation : public filters::FilterBase */ bool update(const T & data_in, T & data_out) override; +protected: + void compute_internal_params() + { + cog_.vector.x = parameters_.CoG.pos[0]; + cog_.vector.y = parameters_.CoG.pos[1]; + cog_.vector.z = parameters_.CoG.pos[2]; + // for now always consider the force is along z + force_z_ = parameters_.CoG.force[2]; + }; + private: rclcpp::Clock::SharedPtr clock_; std::shared_ptr logger_; - std::unique_ptr parameters_; + std::shared_ptr parameter_handler_; + gravity_compensation_filter::Params parameters_; - // Callback for updating dynamic parameters - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; + // Frames for Transformation of Gravity / CoG Vector + std::string world_frame_; + std::string sensor_frame_; + std::string force_frame_; + + // Storage for Calibration Values + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) + double force_z_; // Gravitational Force // Filter objects std::unique_ptr p_tf_Buffer_; @@ -146,22 +99,25 @@ bool GravityCompensation::configure() 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()) + // Initialize the parameters + try { + parameter_handler_ = + std::make_shared(this->params_interface_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); return false; } - - // 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); - }); + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); return true; } diff --git a/include/control_toolbox/parameter_handler.hpp b/include/control_toolbox/parameter_handler.hpp deleted file mode 100644 index f38fd093..00000000 --- a/include/control_toolbox/parameter_handler.hpp +++ /dev/null @@ -1,387 +0,0 @@ -// 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. -// -/// \author: Denis Stogl - -#ifndef CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ -#define CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/node.hpp" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" -#include "rclcpp/parameter_value.hpp" -#include "rcutils/logging_macros.h" - -namespace control_toolbox -{ -using rclcpp::ParameterType; - -namespace impl -{ -inline std::string normalize_params_prefix(std::string prefix) -{ - if (!prefix.empty()) - { - if ('.' != prefix.back()) - { - prefix += '.'; - } - } - return prefix; -} -} // namespace impl - -struct Parameter -{ - Parameter() = default; - - explicit Parameter(const std::string & name, const uint8_t type, const bool configurable = false) - : name(name), type(type), configurable(configurable) - { - } - - std::string name; - uint8_t type; - bool configurable; -}; - -class ParameterHandler -{ -public: - ParameterHandler( - const std::string & params_prefix = "", int nr_bool_params = 0, int nr_integer_params = 0, - int nr_double_params = 0, int nr_string_params = 0, int nr_byte_array_params = 0, - int nr_bool_array_params = 0, int nr_integer_array_params = 0, int nr_double_array_params = 0, - int nr_string_array_params = 0); - - virtual ~ParameterHandler() = default; - - void initialize(rclcpp::Node::SharedPtr node) - { - initialize(node->get_node_parameters_interface(), node->get_logger().get_name()); - } - - void initialize( - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, - const std::string & logger_name = "::ControllerParameters") - { - params_interface_ = node_params; - logger_name_ = logger_name + "." + "parameters"; - } - - void declare_parameters(); - - /** - * Gets all defined parameters from parameter server after parameters are declared. - * Optionally, check parameters' validity and update storage. - * - * \param[in] check_validity check validity after getting parameters (default: **true**). - * \param[in] update update parameters in storage after getting them (default: **true**). - * \return true if all parameters are read successfully, false if a parameter is not provided or - * parameter configuration is wrong. - */ - bool get_parameters(bool check_validity = true, bool update = true); - - /** - * Update all storage variables from the parameter maps. - * Parameters will be only updated if they are previously read from parameter server or dynamic - * reconfigure callback occurred. - * - * \param[in] check_validity check validity before updating parameters (default: **true**). - * \return is update was successful, i.e., parameters are valid. If \p check_validity is set - * **false**, the result is always **true**. - */ - bool update(bool check_validity = true); - - rcl_interfaces::msg::SetParametersResult set_parameter_callback( - const std::vector & parameters); - -protected: - /** - * Override this method to implement custom parameters check. - * Default implementation does not checks anything, always returns true. - * - * \return **true** if parameters are valid, **false** otherwise. - */ - virtual bool check_if_parameters_are_valid() { return true; } - - /** - * Mapping between parameter storage and implementation members. - * The order of parameter in storage is the order of adding parameters to the storage. - * E.g., to access the value of 5-th string parameter added to storage use: - * `fifth_string_param_ = string_parameters_[4].second; - * where `fifth_string_param_` is the name of the member of a child class. - */ - virtual void update_storage() = 0; - -protected: - void add_bool_parameter( - const std::string & name, const bool & configurable = false, const bool & default_value = false) - { - bool_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), - default_value}); - } - - void add_integer_parameter( - const std::string & name, const bool & configurable = false, - const int & default_value = std::numeric_limits::quiet_NaN()) - { - integer_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER, configurable), - default_value}); - } - - void add_double_parameter( - const std::string & name, const bool & configurable = false, - const double & default_value = std::numeric_limits::quiet_NaN()) - { - double_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), - default_value}); - } - - void add_string_parameter( - const std::string & name, const bool & configurable = false, - const std::string & default_value = "") - { - string_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), - default_value}); - } - - void add_byte_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - byte_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BYTE_ARRAY, configurable), - default_value}); - } - - void add_bool_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - bool_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL_ARRAY, configurable), - default_value}); - } - - void add_integer_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - integer_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER_ARRAY, configurable), - default_value}); - } - void add_double_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - double_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), - default_value}); - } - - void add_string_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - string_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), - default_value}); - } - - template - bool empty_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (parameter.second.empty()) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - return empty_numeric_parameter_in_list(parameters); - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - return empty_numeric_parameter_in_list(parameters); - } - - template - bool find_and_assign_parameter_value( - std::vector> & parameter_list, - const rclcpp::Parameter & input_parameter) - { - auto is_in_list = [&](const auto & parameter) { - return parameter.first.name == input_parameter.get_name(); - }; - - auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); - - if (it != parameter_list.end()) - { - if (!it->first.configurable) - { - throw std::runtime_error( - "Parameter " + input_parameter.get_name() + " is not configurable."); - } - else - { - it->second = input_parameter.get_value(); - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), - input_parameter.value_to_string().c_str()); - return true; - } - } - else - { - return false; - } - } - - // Storage members - std::vector> bool_parameters_; - std::vector> integer_parameters_; - std::vector> double_parameters_; - std::vector> string_parameters_; - std::vector>> byte_array_parameters_; - std::vector>> bool_array_parameters_; - std::vector>> integer_array_parameters_; - std::vector>> double_array_parameters_; - std::vector>> string_array_parameters_; - - // Functional members - bool declared_; - bool up_to_date_; - std::string params_prefix_; - - // Input/Output members to ROS 2 - std::string logger_name_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; - -private: - template - void declare_parameters_from_list( - rclcpp::Node::SharedPtr node, const std::vector> & parameters) - { - for (const auto & parameter : parameters) - { - node->declare_parameter(parameter.first.name, parameter.second); - } - } - - template - void declare_parameters_from_list(const std::vector> & parameters) - { - for (const auto & parameter : parameters) - { - // declare parameter only if it does not already exists - if (!params_interface_->has_parameter(parameter.first.name)) - { - rclcpp::ParameterValue default_parameter_value(parameter.second); - rcl_interfaces::msg::ParameterDescriptor descr; - descr.name = parameter.first.name; - descr.type = parameter.first.type; - descr.read_only = false; - - params_interface_->declare_parameter(parameter.first.name, default_parameter_value, descr); - } - } - } - - template - bool empty_numeric_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (std::isnan(parameter.second)) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - template - bool get_parameters_from_list( - const rclcpp::Node::SharedPtr node, std::vector> & parameters) - { - bool ret = true; - for (auto & parameter : parameters) - { - try - { - rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? - ret = node->get_parameter(parameter.first.name, input_parameter); - parameter.second = input_parameter.get_value(); - } - catch (rclcpp::ParameterTypeException & e) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); - ret = false; - break; - } - } - return ret; - } - - template - bool get_parameters_from_list(std::vector> & parameters) - { - bool ret = true; - for (auto & parameter : parameters) - { - try - { - rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? - ret = params_interface_->get_parameter(parameter.first.name, input_parameter); - parameter.second = input_parameter.get_value(); - } - catch (rclcpp::ParameterTypeException & e) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); - ret = false; - break; - } - } - return ret; - } -}; - -} // namespace control_toolbox - -#endif // CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ diff --git a/package.xml b/package.xml index f6a98f64..8446cf48 100644 --- a/package.xml +++ b/package.xml @@ -22,7 +22,7 @@ filters geometry_msgs pluginlib - rcl_interfaces + generate_parameter_library rclcpp rcutils realtime_tools diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index ca5f41f9..f005db56 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -30,16 +30,21 @@ bool GravityCompensation::update( return false; } - parameters_->update(); + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); + } try { transform_ = p_tf_Buffer_->lookupTransform( - parameters_->world_frame_, data_in.header.frame_id, rclcpp::Time()); + 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()); + 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) { @@ -58,13 +63,14 @@ bool GravityCompensation::update( // Transform CoG Vector geometry_msgs::msg::Vector3Stamped cog_transformed; - tf2::doTransform(parameters_->cog_, cog_transformed, transform_cog_); + tf2::doTransform(cog_, cog_transformed, transform_cog_); + // TODO(guihomework): use the full force vector and not only its z component // Compensate for gravity force - temp_force_transformed.vector.z += parameters_->force_z_; + temp_force_transformed.vector.z -= 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); + 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 data_out = data_in; diff --git a/src/control_filters/gravity_compensation_filter_parameters.yaml b/src/control_filters/gravity_compensation_filter_parameters.yaml new file mode 100644 index 00000000..69d9cb68 --- /dev/null +++ b/src/control_filters/gravity_compensation_filter_parameters.yaml @@ -0,0 +1,43 @@ +gravity_compensation_filter: + world_frame: { + type: string, + # default_value: world, + description: "World frame", + read_only: true, + validation: { + not_empty<>: [] + }, + } + sensor_frame: { + type: string, + # default_value: ft_sensor, + description: "Sensor frame", + read_only: true, + validation: { + not_empty<>: [] + }, + } + force_frame: { + type: string, + # default_value: world, + description: "Force frame", + read_only: true, + validation: { + not_empty<>: [] + }, + } + CoG: + pos: { + type: double_array, + description: "Specifies the position of the center of gravity (CoG) of the end effector in the sensor frame.", + validation: { + fixed_size<>: 3 + }, + } + force: { + type: double_array, + description: "Specifies the constant force applied at the CoG of the end effector defined in world frame, e.g [0, 0, -mass * 9.81].", + validation: { + fixed_size<>: 3 + }, + } diff --git a/src/parameter_handler.cpp b/src/parameter_handler.cpp deleted file mode 100644 index 52ca742e..00000000 --- a/src/parameter_handler.cpp +++ /dev/null @@ -1,216 +0,0 @@ -// 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. -// -/// \author: Denis Stogl - -#include "control_toolbox/parameter_handler.hpp" - -#include -#include - -#include "rcutils/logging_macros.h" - -namespace control_toolbox -{ -ParameterHandler::ParameterHandler( - const std::string & params_prefix, int nr_bool_params, int nr_integer_params, - int nr_double_params, int nr_string_params, int nr_byte_array_params, int nr_bool_array_params, - int nr_integer_array_params, int nr_double_array_params, int nr_string_array_params) -: declared_(false), up_to_date_(false), params_prefix_("") -{ - params_prefix_ = impl::normalize_params_prefix(params_prefix); - - bool_parameters_.reserve(nr_bool_params); - integer_parameters_.reserve(nr_integer_params); - double_parameters_.reserve(nr_double_params); - string_parameters_.reserve(nr_string_params); - byte_array_parameters_.reserve(nr_byte_array_params); - bool_array_parameters_.reserve(nr_bool_array_params); - integer_array_parameters_.reserve(nr_integer_array_params); - double_array_parameters_.reserve(nr_double_array_params); - string_array_parameters_.reserve(nr_string_array_params); -} - -void ParameterHandler::declare_parameters() -{ - if (!declared_) - { - declare_parameters_from_list(bool_parameters_); - declare_parameters_from_list(integer_parameters_); - declare_parameters_from_list(double_parameters_); - declare_parameters_from_list(string_parameters_); - declare_parameters_from_list(byte_array_parameters_); - declare_parameters_from_list(bool_array_parameters_); - declare_parameters_from_list(integer_array_parameters_); - declare_parameters_from_list(double_array_parameters_); - declare_parameters_from_list(string_array_parameters_); - - declared_ = true; - } - else - { - RCUTILS_LOG_WARN_NAMED( - logger_name_.c_str(), - "Parameters already declared. Declaration should be done only once. " - "Nothing bad will happen, but please correct your code-logic."); - } -} - -bool ParameterHandler::get_parameters(bool check_validity, bool update) -{ - // TODO(destogl): Should we "auto-declare" parameters here? - if (!declared_) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "Can not get parameters. Please declare them first."); - return false; - } - - bool ret = false; - - // If parameters are updated using dynamic reconfigure callback then there is no need to read - // them again. To ignore multiple manual reads - ret = - get_parameters_from_list(bool_parameters_) && get_parameters_from_list(integer_parameters_) && - get_parameters_from_list(double_parameters_) && get_parameters_from_list(string_parameters_) && - get_parameters_from_list(byte_array_parameters_) && - get_parameters_from_list(bool_array_parameters_) && - get_parameters_from_list(integer_array_parameters_) && - get_parameters_from_list(double_array_parameters_) && - get_parameters_from_list(string_array_parameters_); - - if (ret && check_validity) - { - ret = this->check_if_parameters_are_valid(); - } - - // If it is all good until now the parameters are not up to date anymore - if (ret) - { - up_to_date_ = false; - } - - if (ret && update) - { - ret = this->update(false); - } - - return ret; -} - -bool ParameterHandler::update(bool check_validity) -{ - bool ret = true; - - // Let's make this efficient and execute code only if parameters are updated - if (!up_to_date_) - { - if (check_validity) - { - ret = this->check_if_parameters_are_valid(); - } - - if (ret) - { - this->update_storage(); - } - else - { - RCUTILS_LOG_WARN_NAMED( - logger_name_.c_str(), "Parameters are not valid and therefore will not be updated"); - } - // reset variable to update parameters only when this is needed - up_to_date_ = true; - } - - return ret; -} - -rcl_interfaces::msg::SetParametersResult ParameterHandler::set_parameter_callback( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - // TODO(destogl): this is probably executed in another thread --> mutex protection is needed. - for (const auto & input_parameter : parameters) - { - bool found = false; - - try - { - found = find_and_assign_parameter_value(bool_parameters_, input_parameter); - if (!found) - { - found = find_and_assign_parameter_value(integer_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(double_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(byte_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(bool_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(integer_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(double_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_array_parameters_, input_parameter); - } - - RCUTILS_LOG_INFO_EXPRESSION_NAMED( - found, logger_name_.c_str(), - "Dynamic parameters got changed! Maybe you have to restart controller to update the " - "parameters internally."); - - if (found) - { - up_to_date_ = false; - } - } - catch (const rclcpp::exceptions::InvalidParameterTypeException & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - catch (const std::runtime_error & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - } - - return result; -} - -} // namespace control_toolbox diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index e90b53dd..538890fe 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -13,22 +13,62 @@ // limitations under the License. #include "test_gravity_compensation.hpp" +#include + +TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + node_->declare_parameter("world_frame", "world"); + node_->declare_parameter("sensor_frame", "sensor"); + + // one mandatory param missing, should fail + ASSERT_FALSE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + /* NOTE: one cannot declare or set the missing param afterwards, to then test if configure works, + * because the param is read only and cannot be set anymore. + */ +} + +TEST_F(GravityCompensationTest, TestGravityCompensationInvalidParameters) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + double gravity_acc = 9.81; + double mass = 5.0; + node_->declare_parameter("world_frame", "world"); + node_->declare_parameter("sensor_frame", "sensor"); + node_->declare_parameter("force_frame", "world"); + node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); + + node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0})); + // wrong vector size, should fail + ASSERT_FALSE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.0}))); + // all parameters correctly set + ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} TEST_F(GravityCompensationTest, TestGravityCompensation) { - std::shared_ptr> filter_ = + std::shared_ptr> filter_ = std::make_shared>(); + double gravity_acc = 9.81; + double mass = 5.0; node_->declare_parameter("world_frame", "world"); node_->declare_parameter("sensor_frame", "sensor"); node_->declare_parameter("force_frame", "world"); - 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); + node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0, 0.0})); + node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", - node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); geometry_msgs::msg::WrenchStamped in, out; in.header.frame_id = "world"; @@ -39,7 +79,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) 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.force.z, gravity_acc * mass); ASSERT_EQ(out.wrench.torque.x, 10.0); ASSERT_EQ(out.wrench.torque.y, 0.0); diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp index 7e048e14..daa766df 100644 --- a/test/control_filters/test_gravity_compensation.hpp +++ b/test/control_filters/test_gravity_compensation.hpp @@ -40,7 +40,7 @@ class GravityCompensationTest : public ::testing::Test executor_thread_ = std::thread([this]() { executor_->spin(); }); } - GravityCompensationTest() + GravityCompensationTest() { rclcpp::init(0, nullptr); node_ = std::make_shared("test_gravity_compensation"); From 9cee59e9a6cfb000ca1b22a3164495c85b4f40b3 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 9 Mar 2023 15:20:11 +0100 Subject: [PATCH 09/25] Reworked and activated control filter loading test --- CMakeLists.txt | 4 ++ .../test_load_gravity_compensation.cpp | 57 ++++++++++--------- 2 files changed, 34 insertions(+), 27 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b0e600c9..276f1fa4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,6 +101,10 @@ if(BUILD_TESTING) target_link_libraries(test_gravity_compensation gravity_compensation gravity_compensation_filter_parameters) ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + ament_add_gmock(test_load_gravity_compensation test/control_filters/test_load_gravity_compensation.cpp) + target_link_libraries(test_load_gravity_compensation gravity_compensation gravity_compensation_filter_parameters) + ament_target_dependencies(test_load_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + endif() # Install diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp index 22f66c1e..d32851d2 100644 --- a/test/control_filters/test_load_gravity_compensation.cpp +++ b/test/control_filters/test_load_gravity_compensation.cpp @@ -12,30 +12,33 @@ // 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")); -// } +#include +#include +#include +#include "control_filters/gravity_compensation.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/utilities.hpp" +#include + + +TEST(TestLoadGravityCompensationFilter, load_gravity_compensation_filter_wrench) +{ + rclcpp::init(0, nullptr); + + pluginlib::ClassLoader> + filter_loader("filters", "filters::FilterBase"); + std::shared_ptr> filter; + auto available_classes = filter_loader.getDeclaredClasses(); + std::stringstream sstr; + sstr << "available filters:" << std::endl; + for (const auto& available_class : available_classes) + { + sstr << " " << available_class << std::endl; + } + + std::string filter_type = "control_filters/GravityCompensationWrench"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + + rclcpp::shutdown(); +} From 5715e8f605570d7f404731ac4c23c4d3842bca3b Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 9 Mar 2023 16:08:19 +0100 Subject: [PATCH 10/25] Improved parameter_handler usage when re-configure --- .../control_filters/gravity_compensation.hpp | 31 ++++++++++--------- .../test_gravity_compensation.cpp | 10 ++++-- 2 files changed, 25 insertions(+), 16 deletions(-) diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 9d749376..b93fc1d9 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -100,21 +100,24 @@ bool GravityCompensation::configure() logger_.reset( new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); - // Initialize the parameters - try + // Initialize the parameter_listener once + if (!parameter_handler_) { - parameter_handler_ = - std::make_shared(this->params_interface_); - } - catch (rclcpp::exceptions::ParameterUninitializedException & ex) { - RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; - } - catch (rclcpp::exceptions::InvalidParameterValueException & ex) { - RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; + try + { + parameter_handler_ = + std::make_shared(this->params_interface_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } } parameters_ = parameter_handler_->get_params(); compute_internal_params(); diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index 538890fe..d6df13ad 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -31,7 +31,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) */ } -TEST_F(GravityCompensationTest, TestGravityCompensationInvalidParameters) +TEST_F(GravityCompensationTest, TestGravityCompensationParameters) { std::shared_ptr> filter_ = std::make_shared>(); @@ -49,7 +49,13 @@ TEST_F(GravityCompensationTest, TestGravityCompensationInvalidParameters) node_->get_node_logging_interface(), node_->get_node_parameters_interface())); node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.0}))); - // all parameters correctly set + // all parameters correctly set AND second call to yet unconfigured filter + ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // change a parameter + node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.2}))); + // accept second call to configure with valid parameters to already configured filter ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } From fcb9ccad76c00b8efb0bfec4f79b01e2a07fab5d Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 11 Mar 2023 17:22:09 +0100 Subject: [PATCH 11/25] Applied code-review suggestions and fixed includes --- CMakeLists.txt | 33 ++++++++++--------- README.md | 4 +-- .../control_filters/gravity_compensation.hpp | 2 +- src/control_filters/gravity_compensation.cpp | 2 +- .../test_gravity_compensation.cpp | 2 +- .../test_gravity_compensation.hpp | 2 +- .../test_load_gravity_compensation.cpp | 2 +- 7 files changed, 24 insertions(+), 23 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 276f1fa4..fda8a8d3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,19 +64,29 @@ set(CONTROL_FILTERS_INCLUDE_DEPENDS tf2_ros ) +foreach(Dependency IN ITEMS ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library( + gravity_compensation_filter_parameters + src/control_filters/gravity_compensation_filter_parameters.yaml +) + add_library(gravity_compensation SHARED src/control_filters/gravity_compensation.cpp ) +target_compile_features(gravity_compensation PUBLIC cxx_std_17) target_include_directories(gravity_compensation PUBLIC $ - $ -) -generate_parameter_library( - gravity_compensation_filter_parameters - src/control_filters/gravity_compensation_filter_parameters.yaml + $ ) -target_link_libraries(gravity_compensation gravity_compensation_filter_parameters) -ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + +target_link_libraries(gravity_compensation PUBLIC gravity_compensation_filter_parameters) +ament_target_dependencies(gravity_compensation PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + +# Install pluginlib xml +pluginlib_export_plugin_description_file(filters control_filters.xml) ########## # Testing @@ -124,18 +134,9 @@ install(TARGETS gravity_compensation gravity_compensation_filter_parameters RUNTIME DESTINATION bin ) -# Install pluginlib xml -pluginlib_export_plugin_description_file(filters control_filters.xml) - ament_export_targets(export_control_toolbox HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS}) ament_export_include_directories( include ) -ament_export_libraries( - control_toolbox - gravity_compensation - gravity_compensation_filter_parameters -) - ament_package() diff --git a/README.md b/README.md index 369c15c5..0421232d 100644 --- a/README.md +++ b/README.md @@ -45,7 +45,7 @@ To run it initially over the whole repo you can use: If you get error that something is missing on your computer, do the following for: - - `clang-format-10` + - `clang-format-14` ``` - sudo apt install clang-format-10 + sudo apt install clang-format-14 ``` diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index b93fc1d9..199211e7 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index f005db56..9aae34c2 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index d6df13ad..05ced9ef 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp index daa766df..51eb9a8b 100644 --- a/test/control_filters/test_gravity_compensation.hpp +++ b/test/control_filters/test_gravity_compensation.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp index d32851d2..12abf276 100644 --- a/test/control_filters/test_load_gravity_compensation.cpp +++ b/test/control_filters/test_load_gravity_compensation.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From b18ffbdc7906ee3d39443818f4993b8b6a0ea247 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 13 Mar 2023 20:08:16 +0100 Subject: [PATCH 12/25] Fixed missing namespace forwarding --- include/control_filters/gravity_compensation.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 199211e7..135ad199 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -106,7 +106,8 @@ bool GravityCompensation::configure() try { parameter_handler_ = - std::make_shared(this->params_interface_); + std::make_shared(this->params_interface_, + this->param_prefix_); } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); From ebd7481bfbcfd3e0dbb1a04dff24dc777f8d7a4b Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 15 Mar 2023 21:41:47 +0100 Subject: [PATCH 13/25] Switched to wrench transforms --- src/control_filters/gravity_compensation.cpp | 30 ++++++-------------- 1 file changed, 9 insertions(+), 21 deletions(-) diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 9aae34c2..3f0022f4 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -51,36 +51,24 @@ bool GravityCompensation::update( 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; + // Transform data_in to world_frame frame + geometry_msgs::msg::Wrench wrench_world; + tf2::doTransform(data_in.wrench, wrench_world, transform_); - // 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 + // Transform CoG Vector to world_frame frame geometry_msgs::msg::Vector3Stamped cog_transformed; tf2::doTransform(cog_, cog_transformed, transform_cog_); // TODO(guihomework): use the full force vector and not only its z component // Compensate for gravity force - temp_force_transformed.vector.z -= force_z_; + wrench_world.force.z -= force_z_; // Compensation Values for torque result from cross-product 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); - + wrench_world.torque.x -= (force_z_ * cog_transformed.vector.y); + wrench_world.torque.y += (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; + // Transform wrench_world to data_in frame id + tf2::doTransform(wrench_world, data_out.wrench, transform_back_); return true; } From b7ffc66fbd19be3408a029c59a907727950413ea Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 15 Mar 2023 21:42:40 +0100 Subject: [PATCH 14/25] Fail update at failed lookupTransform --- src/control_filters/gravity_compensation.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 3f0022f4..085ae9c7 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -49,6 +49,7 @@ bool GravityCompensation::update( catch (const tf2::TransformException & ex) { RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 5000, "%s", ex.what()); + return false; // if cannot transform, result of subsequent computations is invalid } // Transform data_in to world_frame frame From 94ed150acc14f2622b1479f70b2b93db2789a190 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 15 Mar 2023 21:53:36 +0100 Subject: [PATCH 15/25] Allow data_out frame of reference selection --- src/control_filters/gravity_compensation.cpp | 19 ++++++++++++++----- .../test_gravity_compensation.cpp | 6 ++++++ 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 085ae9c7..19037eb1 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -41,8 +41,19 @@ bool GravityCompensation::update( { 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()); + // use data_out frame id for the back transformation, otherwise same is data_in + if (!data_out.header.frame_id.empty()) + { + data_out.header.stamp = data_in.header.stamp; // only copy the timestamp + transform_back_ = p_tf_Buffer_->lookupTransform( + data_out.header.frame_id, parameters_.world_frame, rclcpp::Time()); + } + else + { + data_out.header = data_in.header; // keep the same header and same frame_id + 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()); } @@ -66,9 +77,7 @@ bool GravityCompensation::update( // Compensation Values for torque result from cross-product of cog Vector and (0 0 G) wrench_world.torque.x -= (force_z_ * cog_transformed.vector.y); wrench_world.torque.y += (force_z_ * cog_transformed.vector.x); - // Copy Message and Compensate values for Gravity Force and Resulting Torque - data_out = data_in; - // Transform wrench_world to data_in frame id + // Transform wrench_world to data_out frame_id if not empty otherwise to data_in frame id tf2::doTransform(wrench_world, data_out.wrench, transform_back_); return true; diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index 05ced9ef..b98ae626 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -90,4 +90,10 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) ASSERT_EQ(out.wrench.torque.x, 10.0); ASSERT_EQ(out.wrench.torque.y, 0.0); ASSERT_EQ(out.wrench.torque.z, 0.0); + + out.header.frame_id = "base"; + // should fail due to missing transform for desired output frame + ASSERT_FALSE(filter_->update(in, out)); + + // TODO(guihomework) Add a test with real lookups } From fc47ee09acc3db924d7e699418463df07d44c225 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 15 Mar 2023 21:54:24 +0100 Subject: [PATCH 16/25] Renaming test nodes for consistency --- test/control_filters/test_gravity_compensation.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index b98ae626..3a8b4221 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -24,7 +24,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) node_->declare_parameter("sensor_frame", "sensor"); // one mandatory param missing, should fail - ASSERT_FALSE(filter_->configure("", "TestGravityCompensationFilter", + ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); /* NOTE: one cannot declare or set the missing param afterwards, to then test if configure works, * because the param is read only and cannot be set anymore. @@ -45,18 +45,18 @@ TEST_F(GravityCompensationTest, TestGravityCompensationParameters) node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0})); // wrong vector size, should fail - ASSERT_FALSE(filter_->configure("", "TestGravityCompensationFilter", + ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.0}))); // all parameters correctly set AND second call to yet unconfigured filter - ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); // change a parameter node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.2}))); // accept second call to configure with valid parameters to already configured filter - ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } @@ -73,7 +73,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0, 0.0})); node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); - ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); geometry_msgs::msg::WrenchStamped in, out; From c91003314853b15576dbb5f51d914b86eb0ca763 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 16 Mar 2023 17:04:13 +0100 Subject: [PATCH 17/25] Made computation clearer & for generic const force --- .../control_filters/gravity_compensation.hpp | 16 ++++---- src/control_filters/gravity_compensation.cpp | 38 +++++++++++++------ ...ravity_compensation_filter_parameters.yaml | 6 +-- .../test_gravity_compensation.cpp | 4 ++ 4 files changed, 42 insertions(+), 22 deletions(-) diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 135ad199..21d242f3 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -55,8 +55,9 @@ class GravityCompensation : public filters::FilterBase cog_.vector.x = parameters_.CoG.pos[0]; cog_.vector.y = parameters_.CoG.pos[1]; cog_.vector.z = parameters_.CoG.pos[2]; - // for now always consider the force is along z - force_z_ = parameters_.CoG.force[2]; + force_.vector.x = parameters_.CoG.force[0]; + force_.vector.y = parameters_.CoG.force[1]; + force_.vector.z = parameters_.CoG.force[2]; }; private: @@ -66,18 +67,19 @@ class GravityCompensation : public filters::FilterBase gravity_compensation_filter::Params parameters_; // Frames for Transformation of Gravity / CoG Vector - std::string world_frame_; - std::string sensor_frame_; - std::string force_frame_; + std::string world_frame_; // frame in which computation occur + std::string sensor_frame_; // frame in which Cog is given + std::string force_frame_; // frame in which external force is given // 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 force_; // Gravity Force Vector (wrt Force Frame) // Filter objects std::unique_ptr p_tf_Buffer_; std::unique_ptr p_tf_Listener_; - geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; + geometry_msgs::msg::TransformStamped transform_datain_world_, transform_world_dataout_, + transform_cog_world_, transform_force_world_; }; template diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 19037eb1..ace00d2b 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -17,6 +17,7 @@ #include "geometry_msgs/msg/vector3_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Vector3.h" namespace control_filters { @@ -39,23 +40,25 @@ bool GravityCompensation::update( try { - transform_ = p_tf_Buffer_->lookupTransform( + transform_datain_world_ = p_tf_Buffer_->lookupTransform( parameters_.world_frame, data_in.header.frame_id, rclcpp::Time()); // use data_out frame id for the back transformation, otherwise same is data_in if (!data_out.header.frame_id.empty()) { data_out.header.stamp = data_in.header.stamp; // only copy the timestamp - transform_back_ = p_tf_Buffer_->lookupTransform( + transform_world_dataout_ = p_tf_Buffer_->lookupTransform( data_out.header.frame_id, parameters_.world_frame, rclcpp::Time()); } else { data_out.header = data_in.header; // keep the same header and same frame_id - transform_back_ = p_tf_Buffer_->lookupTransform( + transform_world_dataout_ = p_tf_Buffer_->lookupTransform( data_in.header.frame_id, parameters_.world_frame, rclcpp::Time()); } - transform_cog_ = p_tf_Buffer_->lookupTransform( + transform_force_world_ = p_tf_Buffer_->lookupTransform( parameters_.world_frame, parameters_.force_frame, rclcpp::Time()); + transform_cog_world_ = p_tf_Buffer_->lookupTransform( + parameters_.world_frame, parameters_.sensor_frame, rclcpp::Time()); } catch (const tf2::TransformException & ex) { @@ -65,20 +68,31 @@ bool GravityCompensation::update( // Transform data_in to world_frame frame geometry_msgs::msg::Wrench wrench_world; - tf2::doTransform(data_in.wrench, wrench_world, transform_); + tf2::doTransform(data_in.wrench, wrench_world, transform_datain_world_); // Transform CoG Vector to world_frame frame geometry_msgs::msg::Vector3Stamped cog_transformed; - tf2::doTransform(cog_, cog_transformed, transform_cog_); + tf2::doTransform(cog_, cog_transformed, transform_cog_world_); + + // Transform Ext force vector to world_frame frame + geometry_msgs::msg::Vector3Stamped force_transformed; + tf2::doTransform(force_, force_transformed, transform_force_world_); - // TODO(guihomework): use the full force vector and not only its z component // Compensate for gravity force - wrench_world.force.z -= force_z_; - // Compensation Values for torque result from cross-product of cog Vector and (0 0 G) - wrench_world.torque.x -= (force_z_ * cog_transformed.vector.y); - wrench_world.torque.y += (force_z_ * cog_transformed.vector.x); + wrench_world.force.x -= force_transformed.vector.x; + wrench_world.force.y -= force_transformed.vector.y; + wrench_world.force.z -= force_transformed.vector.z; + // Compensation values for torque result from cross-product of cog Vector and force + tf2::Vector3 cog_vector = {cog_transformed.vector.x, cog_transformed.vector.y, + cog_transformed.vector.z}; + auto added_torque = cog_vector.cross({force_transformed.vector.x, + force_transformed.vector.y, + force_transformed.vector.z}); + wrench_world.torque.x -= added_torque.getX(); + wrench_world.torque.y -= added_torque.getY(); + wrench_world.torque.z -= added_torque.getZ(); // Transform wrench_world to data_out frame_id if not empty otherwise to data_in frame id - tf2::doTransform(wrench_world, data_out.wrench, transform_back_); + tf2::doTransform(wrench_world, data_out.wrench, transform_world_dataout_); return true; } diff --git a/src/control_filters/gravity_compensation_filter_parameters.yaml b/src/control_filters/gravity_compensation_filter_parameters.yaml index 69d9cb68..6fbe0dc9 100644 --- a/src/control_filters/gravity_compensation_filter_parameters.yaml +++ b/src/control_filters/gravity_compensation_filter_parameters.yaml @@ -11,7 +11,7 @@ gravity_compensation_filter: sensor_frame: { type: string, # default_value: ft_sensor, - description: "Sensor frame", + description: "Sensor frame in which Cog is measured", read_only: true, validation: { not_empty<>: [] @@ -20,7 +20,7 @@ gravity_compensation_filter: force_frame: { type: string, # default_value: world, - description: "Force frame", + description: "Force frame in which the external force is given", read_only: true, validation: { not_empty<>: [] @@ -36,7 +36,7 @@ gravity_compensation_filter: } force: { type: double_array, - description: "Specifies the constant force applied at the CoG of the end effector defined in world frame, e.g [0, 0, -mass * 9.81].", + description: "Specifies the constant force applied at the CoG of the end effector defined in force frame, e.g [0, 0, -mass * 9.81].", validation: { fixed_size<>: 3 }, diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index 3a8b4221..5187d890 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -81,6 +81,10 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) in.wrench.force.x = 1.0; in.wrench.torque.x = 10.0; + // should fail due to missing sensor frame to world transform + ASSERT_FALSE(filter_->update(in, out)); + node_->set_parameter(rclcpp::Parameter("sensor_frame", "world")); + // should pass (now transform is identity) ASSERT_TRUE(filter_->update(in, out)); ASSERT_EQ(out.wrench.force.x, 1.0); From 8fec166c49cb2fd1862cd87e6744fefd731af1e1 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 16 Mar 2023 21:48:21 +0100 Subject: [PATCH 18/25] Clarified error msg of GravityCompensation update --- src/control_filters/gravity_compensation.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index ace00d2b..b9b4b299 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -62,7 +62,12 @@ bool GravityCompensation::update( } catch (const tf2::TransformException & ex) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 5000, "%s", ex.what()); + std::stringstream frames_sstr; + frames_sstr << "datain:" << data_in.header.frame_id << ", dataout:" << data_out.header.frame_id; + frames_sstr << ", world:" << parameters_.world_frame << ", force:" << parameters_.force_frame; + frames_sstr << ", sensor:" << parameters_.sensor_frame; + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 5000, "GravityCompensation update failed:%s,\ + given frames are %s", ex.what(), frames_sstr.str().c_str()); return false; // if cannot transform, result of subsequent computations is invalid } From 73072fb70a46047259ea56fc707f1e544878988b Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 18 Mar 2023 11:35:43 +0100 Subject: [PATCH 19/25] Fixed, simplified & clarified Gravity computations --- .../control_filters/gravity_compensation.hpp | 20 +++--- src/control_filters/gravity_compensation.cpp | 68 +++++++++---------- ...ravity_compensation_filter_parameters.yaml | 17 ++--- .../test_gravity_compensation.cpp | 5 +- 4 files changed, 46 insertions(+), 64 deletions(-) diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 21d242f3..d1db2367 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -55,9 +55,9 @@ class GravityCompensation : public filters::FilterBase cog_.vector.x = parameters_.CoG.pos[0]; cog_.vector.y = parameters_.CoG.pos[1]; cog_.vector.z = parameters_.CoG.pos[2]; - force_.vector.x = parameters_.CoG.force[0]; - force_.vector.y = parameters_.CoG.force[1]; - force_.vector.z = parameters_.CoG.force[2]; + cst_ext_force_.vector.x = parameters_.CoG.force[0]; + cst_ext_force_.vector.y = parameters_.CoG.force[1]; + cst_ext_force_.vector.z = parameters_.CoG.force[2]; }; private: @@ -67,19 +67,17 @@ class GravityCompensation : public filters::FilterBase gravity_compensation_filter::Params parameters_; // Frames for Transformation of Gravity / CoG Vector - std::string world_frame_; // frame in which computation occur - std::string sensor_frame_; // frame in which Cog is given - std::string force_frame_; // frame in which external force is given - + std::string world_frame_; // frame in which gravity is given + std::string sensor_frame_; // frame in which Cog is given and compution occur // Storage for Calibration Values - geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) - geometry_msgs::msg::Vector3Stamped force_; // Gravity Force Vector (wrt Force Frame) + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt sensor frame) + geometry_msgs::msg::Vector3Stamped cst_ext_force_; // Gravity Force Vector (wrt world frame) // Filter objects std::unique_ptr p_tf_Buffer_; std::unique_ptr p_tf_Listener_; - geometry_msgs::msg::TransformStamped transform_datain_world_, transform_world_dataout_, - transform_cog_world_, transform_force_world_; + geometry_msgs::msg::TransformStamped transform_sensor_datain_, transform_world_dataout_, + transform_data_out_sensor_, rot_sensor_world_; }; template diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index b9b4b299..6b03061a 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -40,64 +40,60 @@ bool GravityCompensation::update( try { - transform_datain_world_ = p_tf_Buffer_->lookupTransform( - parameters_.world_frame, data_in.header.frame_id, rclcpp::Time()); + // transform from data_in frame to sensor_frame (most likely to be identity) + transform_sensor_datain_ = p_tf_Buffer_->lookupTransform(parameters_.sensor_frame, + data_in.header.frame_id, rclcpp::Time()); + // use data_out frame id for the back transformation, otherwise same is data_in if (!data_out.header.frame_id.empty()) { data_out.header.stamp = data_in.header.stamp; // only copy the timestamp - transform_world_dataout_ = p_tf_Buffer_->lookupTransform( - data_out.header.frame_id, parameters_.world_frame, rclcpp::Time()); } else { data_out.header = data_in.header; // keep the same header and same frame_id - transform_world_dataout_ = p_tf_Buffer_->lookupTransform( - data_in.header.frame_id, parameters_.world_frame, rclcpp::Time()); } - transform_force_world_ = p_tf_Buffer_->lookupTransform( - parameters_.world_frame, parameters_.force_frame, rclcpp::Time()); - transform_cog_world_ = p_tf_Buffer_->lookupTransform( - parameters_.world_frame, parameters_.sensor_frame, rclcpp::Time()); + transform_data_out_sensor_ = p_tf_Buffer_->lookupTransform( + data_out.header.frame_id, parameters_.sensor_frame, rclcpp::Time()); + // rotation only (because gravity is a field) from world (gravity frame) to sensor frame + rot_sensor_world_ = p_tf_Buffer_->lookupTransform( + parameters_.sensor_frame, parameters_.world_frame, rclcpp::Time()); } catch (const tf2::TransformException & ex) { std::stringstream frames_sstr; - frames_sstr << "datain:" << data_in.header.frame_id << ", dataout:" << data_out.header.frame_id; - frames_sstr << ", world:" << parameters_.world_frame << ", force:" << parameters_.force_frame; - frames_sstr << ", sensor:" << parameters_.sensor_frame; - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 5000, "GravityCompensation update failed:%s,\ - given frames are %s", ex.what(), frames_sstr.str().c_str()); + frames_sstr << "datain:" << data_in.header.frame_id << " dataout:" << data_out.header.frame_id; + frames_sstr << " world:" << parameters_.world_frame << " sensor:" << parameters_.sensor_frame; + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 5000, + "GravityCompensation update failed:%s, given frames are %s", ex.what(), + frames_sstr.str().c_str()); return false; // if cannot transform, result of subsequent computations is invalid } - // Transform data_in to world_frame frame - geometry_msgs::msg::Wrench wrench_world; - tf2::doTransform(data_in.wrench, wrench_world, transform_datain_world_); + // Transform data_in to sensor_frame frame + geometry_msgs::msg::Wrench wrench_sensor; + tf2::doTransform(data_in.wrench, wrench_sensor, transform_sensor_datain_); - // Transform CoG Vector to world_frame frame - geometry_msgs::msg::Vector3Stamped cog_transformed; - tf2::doTransform(cog_, cog_transformed, transform_cog_world_); + // CoG is already in sensor_frame - // Transform Ext force vector to world_frame frame - geometry_msgs::msg::Vector3Stamped force_transformed; - tf2::doTransform(force_, force_transformed, transform_force_world_); + // Transform field force vector to sensor_frame frame + geometry_msgs::msg::Vector3Stamped cst_ext_force_transformed; + tf2::doTransform(cst_ext_force_, cst_ext_force_transformed, rot_sensor_world_); // Compensate for gravity force - wrench_world.force.x -= force_transformed.vector.x; - wrench_world.force.y -= force_transformed.vector.y; - wrench_world.force.z -= force_transformed.vector.z; + wrench_sensor.force.x -= cst_ext_force_transformed.vector.x; + wrench_sensor.force.y -= cst_ext_force_transformed.vector.y; + wrench_sensor.force.z -= cst_ext_force_transformed.vector.z; // Compensation values for torque result from cross-product of cog Vector and force - tf2::Vector3 cog_vector = {cog_transformed.vector.x, cog_transformed.vector.y, - cog_transformed.vector.z}; - auto added_torque = cog_vector.cross({force_transformed.vector.x, - force_transformed.vector.y, - force_transformed.vector.z}); - wrench_world.torque.x -= added_torque.getX(); - wrench_world.torque.y -= added_torque.getY(); - wrench_world.torque.z -= added_torque.getZ(); + tf2::Vector3 cog_vector = {cog_.vector.x, cog_.vector.y, cog_.vector.z}; + auto added_torque = cog_vector.cross({cst_ext_force_transformed.vector.x, + cst_ext_force_transformed.vector.y, + cst_ext_force_transformed.vector.z}); + wrench_sensor.torque.x -= added_torque.getX(); + wrench_sensor.torque.y -= added_torque.getY(); + wrench_sensor.torque.z -= added_torque.getZ(); // Transform wrench_world to data_out frame_id if not empty otherwise to data_in frame id - tf2::doTransform(wrench_world, data_out.wrench, transform_world_dataout_); + tf2::doTransform(wrench_sensor, data_out.wrench, transform_data_out_sensor_); return true; } diff --git a/src/control_filters/gravity_compensation_filter_parameters.yaml b/src/control_filters/gravity_compensation_filter_parameters.yaml index 6fbe0dc9..95cfe8b0 100644 --- a/src/control_filters/gravity_compensation_filter_parameters.yaml +++ b/src/control_filters/gravity_compensation_filter_parameters.yaml @@ -2,7 +2,7 @@ gravity_compensation_filter: world_frame: { type: string, # default_value: world, - description: "World frame", + description: "fixed world frame in which the constant field-induced force is given", read_only: true, validation: { not_empty<>: [] @@ -11,16 +11,7 @@ gravity_compensation_filter: sensor_frame: { type: string, # default_value: ft_sensor, - description: "Sensor frame in which Cog is measured", - read_only: true, - validation: { - not_empty<>: [] - }, - } - force_frame: { - type: string, - # default_value: world, - description: "Force frame in which the external force is given", + description: "Sensor frame in which center of gravity (CoG) is measured and computation occur", read_only: true, validation: { not_empty<>: [] @@ -29,14 +20,14 @@ gravity_compensation_filter: CoG: pos: { type: double_array, - description: "Specifies the position of the center of gravity (CoG) of the end effector in the sensor frame.", + description: "Position of the CoG of tool attached to the FT sensor in the sensor frame.", validation: { fixed_size<>: 3 }, } force: { type: double_array, - description: "Specifies the constant force applied at the CoG of the end effector defined in force frame, e.g [0, 0, -mass * 9.81].", + description: "Specifies the constant field-induced force applied at the CoG of tool defined in fixed world frame, e.g [0, 0, -mass * 9.81].", validation: { fixed_size<>: 3 }, diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index 5187d890..987c560a 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -21,7 +21,6 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) std::make_shared>(); node_->declare_parameter("world_frame", "world"); - node_->declare_parameter("sensor_frame", "sensor"); // one mandatory param missing, should fail ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", @@ -40,7 +39,6 @@ TEST_F(GravityCompensationTest, TestGravityCompensationParameters) double mass = 5.0; node_->declare_parameter("world_frame", "world"); node_->declare_parameter("sensor_frame", "sensor"); - node_->declare_parameter("force_frame", "world"); node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0})); @@ -69,7 +67,6 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) double mass = 5.0; node_->declare_parameter("world_frame", "world"); node_->declare_parameter("sensor_frame", "sensor"); - node_->declare_parameter("force_frame", "world"); node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0, 0.0})); node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); @@ -81,7 +78,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) in.wrench.force.x = 1.0; in.wrench.torque.x = 10.0; - // should fail due to missing sensor frame to world transform + // should fail due to missing datain frame to sensor frame transform ASSERT_FALSE(filter_->update(in, out)); node_->set_parameter(rclcpp::Parameter("sensor_frame", "world")); // should pass (now transform is identity) From fdf43dbf2b4afb1b3a50a075b2cb0c06e3ae50ab Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 20 Mar 2023 10:44:04 +0100 Subject: [PATCH 20/25] Cleaned-up --- CMakeLists.txt | 5 +---- test/control_filters/test_gravity_compensation.hpp | 3 --- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fda8a8d3..ee02b5cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -135,8 +135,5 @@ install(TARGETS gravity_compensation gravity_compensation_filter_parameters ) ament_export_targets(export_control_toolbox HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS}) -ament_export_include_directories( - include -) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp index 51eb9a8b..14e24129 100644 --- a/test/control_filters/test_gravity_compensation.hpp +++ b/test/control_filters/test_gravity_compensation.hpp @@ -28,9 +28,6 @@ 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: From b74a1eeb71ffc6da3501a31219e56598218c7186 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 20 Mar 2023 10:44:24 +0100 Subject: [PATCH 21/25] Improved understandability of the filter --- .../control_filters/gravity_compensation.hpp | 2 +- src/control_filters/README.md | 59 +++++++++++++++++++ src/control_filters/gravity_compensation.cpp | 15 ++--- 3 files changed, 68 insertions(+), 8 deletions(-) create mode 100644 src/control_filters/README.md diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index d1db2367..5dbd4f15 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -77,7 +77,7 @@ class GravityCompensation : public filters::FilterBase std::unique_ptr p_tf_Buffer_; std::unique_ptr p_tf_Listener_; geometry_msgs::msg::TransformStamped transform_sensor_datain_, transform_world_dataout_, - transform_data_out_sensor_, rot_sensor_world_; + transform_data_out_sensor_, transform_sensor_world_; }; template diff --git a/src/control_filters/README.md b/src/control_filters/README.md new file mode 100644 index 00000000..fa304cb6 --- /dev/null +++ b/src/control_filters/README.md @@ -0,0 +1,59 @@ +# Control Filters + +## Available filters + +* Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench). +* + + +## Gravity Compensation filter + + This filter implements a gravity compensation algorithm, applied to an `data_in wrench`, computed at a `sensor frame` in which the center of gravity (CoG) of the to-be-compensated mass is known. + + The filter relies on ROS TF, and might fail if transforms are missing. + + Note that, for convenience, the filter can perform additional frame changes if data_out frame id is given. + +### required parameters + +* `world_frame` (ℛw): frame in which the `CoG.force` is represented. +* `sensor_frame` (ℛs): frame in which the `CoG.pos` is defined +* `CoG.pos` (ps): position of the CoG of the mass the filter should compensate for +* `CoG.force` (gw): constant (but updatable) force of gravity at the Cog (typically m.G), defined along axes of the `world_frame` + +### algorithm + +Given +* above-required parameters, ℛw, ℛs, ps, gw +* `data_in`, a wrench ℱi = {fi, τi} represented in `data_in.frame` ℛi +* access to TF homogeneous transforms: + * Tsi from ℛi to ℛs + * Tsw from ℛw to ℛs + * Tos from ℛs to ℛo + +Compute `data_out` compensated wrench ℱco = {fco, τco} represented in `data_out.frame` ℛo if given, or `data_in.frame` ℛi otherwise, with equations: + +ℱco = Tos.ℱcs, + + +with ℱcs = {fcs, τcs} the compensated wrench in `sensor_frame` (common frame for computation) + +and, + +fcs = fs - Tsw.gw + +its force and, + +τcs = τs - ps x (Tsw.gw) + +its torque, and + +ℱs = Tsi.ℱi + +the full transform of the input wrench ℱi to sensor frame ℛs + +Remarks : +* a full vector is used for gravity force, to not impose gravity to be only along z of `world_frame`. +* `data_in.frame` is usually equal to `sensor_frame`, but could be different since measurement of wrech might occur in another frame. Ex: measurements are at the **FT sensor flange** = `data_in.frame`, but CoG is given in **FT sensor base** = `sensor_frame` (=frame to which it is mounted on the robot), introducing an offset (thichkess of the sensor) to be accounted for. +* `data_out.frame` is usually `data_in.frame`, but for convenience, can be set to any other useful frame. Ex: Wrench expressed in a `control_frame` for instance center of a gripper. +* Tsw will only rotate the gw vector, because gravity is a field applied everywhere, and not a wrench (no torque should be induced by transforming from ℛw to ℛs). diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 6b03061a..b525cdad 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -40,7 +40,7 @@ bool GravityCompensation::update( try { - // transform from data_in frame to sensor_frame (most likely to be identity) + // transform from data_in frame to sensor_frame transform_sensor_datain_ = p_tf_Buffer_->lookupTransform(parameters_.sensor_frame, data_in.header.frame_id, rclcpp::Time()); @@ -55,8 +55,8 @@ bool GravityCompensation::update( } transform_data_out_sensor_ = p_tf_Buffer_->lookupTransform( data_out.header.frame_id, parameters_.sensor_frame, rclcpp::Time()); - // rotation only (because gravity is a field) from world (gravity frame) to sensor frame - rot_sensor_world_ = p_tf_Buffer_->lookupTransform( + // transform from world (gravity) frame to sensor frame + transform_sensor_world_ = p_tf_Buffer_->lookupTransform( parameters_.sensor_frame, parameters_.world_frame, rclcpp::Time()); } catch (const tf2::TransformException & ex) @@ -76,15 +76,16 @@ bool GravityCompensation::update( // CoG is already in sensor_frame - // Transform field force vector to sensor_frame frame + // Rotate (no wrench, just a force) the gravity force to sensor frame geometry_msgs::msg::Vector3Stamped cst_ext_force_transformed; - tf2::doTransform(cst_ext_force_, cst_ext_force_transformed, rot_sensor_world_); + tf2::doTransform(cst_ext_force_, cst_ext_force_transformed, transform_sensor_world_); - // Compensate for gravity force + // Compensate for gravity force in sensor frame wrench_sensor.force.x -= cst_ext_force_transformed.vector.x; wrench_sensor.force.y -= cst_ext_force_transformed.vector.y; wrench_sensor.force.z -= cst_ext_force_transformed.vector.z; - // Compensation values for torque result from cross-product of cog Vector and force + // Compensate for torque produced by offset CoG in sensor frame + // result from cross-product of cog Vector and force tf2::Vector3 cog_vector = {cog_.vector.x, cog_.vector.y, cog_.vector.z}; auto added_torque = cog_vector.cross({cst_ext_force_transformed.vector.x, cst_ext_force_transformed.vector.y, From 88d6b8a7a25b4814a245b2a335a6b9757afcfbef Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 25 Mar 2023 19:25:44 +0100 Subject: [PATCH 22/25] Switched to yaml-based param in filter test --- CMakeLists.txt | 4 +- .../test_gravity_compensation.cpp | 53 ++++++++++++------- .../test_gravity_compensation.hpp | 5 +- .../test_gravity_compensation_parameters.yaml | 35 ++++++++++++ 4 files changed, 73 insertions(+), 24 deletions(-) create mode 100644 test/control_filters/test_gravity_compensation_parameters.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index ee02b5cf..39531a87 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -107,7 +107,9 @@ if(BUILD_TESTING) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) ## Control Filters - ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) + add_rostest_with_parameters_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_gravity_compensation_parameters.yaml + ) target_link_libraries(test_gravity_compensation gravity_compensation gravity_compensation_filter_parameters) ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index 987c560a..39ac1055 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -20,8 +20,6 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) std::shared_ptr> filter_ = std::make_shared>(); - node_->declare_parameter("world_frame", "world"); - // one mandatory param missing, should fail ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); @@ -30,22 +28,16 @@ TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) */ } -TEST_F(GravityCompensationTest, TestGravityCompensationParameters) +TEST_F(GravityCompensationTest, TestGravityCompensationInvalidThenFixedParameter) { std::shared_ptr> filter_ = std::make_shared>(); - double gravity_acc = 9.81; - double mass = 5.0; - node_->declare_parameter("world_frame", "world"); - node_->declare_parameter("sensor_frame", "sensor"); - node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); - - node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0})); // wrong vector size, should fail ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + // fixed wrong vector size node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.0}))); // all parameters correctly set AND second call to yet unconfigured filter ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", @@ -58,17 +50,32 @@ TEST_F(GravityCompensationTest, TestGravityCompensationParameters) node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } -TEST_F(GravityCompensationTest, TestGravityCompensation) + +TEST_F(GravityCompensationTest, TestGravityCompensationMissingTransform) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // all parameters correctly set + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + // should fail due to missing datain frame to sensor frame transform + ASSERT_FALSE(filter_->update(in, out)); +} + +TEST_F(GravityCompensationTest, TestGravityCompensationComputation) { std::shared_ptr> filter_ = std::make_shared>(); double gravity_acc = 9.81; double mass = 5.0; - node_->declare_parameter("world_frame", "world"); - node_->declare_parameter("sensor_frame", "sensor"); - node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0, 0.0})); - node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); @@ -78,15 +85,12 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) in.wrench.force.x = 1.0; in.wrench.torque.x = 10.0; - // should fail due to missing datain frame to sensor frame transform - ASSERT_FALSE(filter_->update(in, out)); - node_->set_parameter(rclcpp::Parameter("sensor_frame", "world")); - // should pass (now transform is identity) + // should pass (transform is identity) ASSERT_TRUE(filter_->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, gravity_acc * mass); + ASSERT_NEAR(out.wrench.force.z, gravity_acc * mass, 0.0001); ASSERT_EQ(out.wrench.torque.x, 10.0); ASSERT_EQ(out.wrench.torque.y, 0.0); @@ -98,3 +102,12 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) // TODO(guihomework) Add a test with real lookups } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp index 14e24129..367811d6 100644 --- a/test/control_filters/test_gravity_compensation.hpp +++ b/test/control_filters/test_gravity_compensation.hpp @@ -33,14 +33,14 @@ class GravityCompensationTest : public ::testing::Test public: void SetUp() override { + auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + node_ = std::make_shared(testname); executor_->add_node(node_); executor_thread_ = std::thread([this]() { executor_->spin(); }); } GravityCompensationTest() { - rclcpp::init(0, nullptr); - node_ = std::make_shared("test_gravity_compensation"); executor_ = std::make_shared(); } @@ -52,7 +52,6 @@ class GravityCompensationTest : public ::testing::Test executor_thread_.join(); } node_.reset(); - rclcpp::shutdown(); } protected: diff --git a/test/control_filters/test_gravity_compensation_parameters.yaml b/test/control_filters/test_gravity_compensation_parameters.yaml new file mode 100644 index 00000000..1224e284 --- /dev/null +++ b/test/control_filters/test_gravity_compensation_parameters.yaml @@ -0,0 +1,35 @@ +TestGravityCompensationAllParameters: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0] + +TestGravityCompensationMissingParameters: + ros__parameters: + world_frame: world + +TestGravityCompensationInvalidThenFixedParameter: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0] + +TestGravityCompensationMissingTransform: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0] + +TestGravityCompensationComputation: + ros__parameters: + world_frame: world + sensor_frame: world + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0] From b22a7549ece5c7d7c44089f71c71b505514408a3 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Apr 2023 11:29:57 +0200 Subject: [PATCH 23/25] Applied suggested formatting and typo fixes --- .../control_filters/gravity_compensation.hpp | 1 - src/control_filters/README.md | 30 +++++++++---------- ...ravity_compensation_filter_parameters.yaml | 6 ++-- 3 files changed, 16 insertions(+), 21 deletions(-) diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 5dbd4f15..adfa8f00 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -29,7 +29,6 @@ namespace control_filters { - template class GravityCompensation : public filters::FilterBase { diff --git a/src/control_filters/README.md b/src/control_filters/README.md index fa304cb6..02cb7c6e 100644 --- a/src/control_filters/README.md +++ b/src/control_filters/README.md @@ -1,37 +1,35 @@ -# Control Filters +# Control filters ## Available filters * Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench). -* +## Gravity compensation filter -## Gravity Compensation filter + This filter implements a gravity compensation algorithm, applied to an `data_in` wrench, computed at a `sensor_frame` in which the center of gravity (CoG) of the to-be-compensated mass is known. - This filter implements a gravity compensation algorithm, applied to an `data_in wrench`, computed at a `sensor frame` in which the center of gravity (CoG) of the to-be-compensated mass is known. - - The filter relies on ROS TF, and might fail if transforms are missing. + The filter relies on tf2, and might fail if transforms are missing. Note that, for convenience, the filter can perform additional frame changes if data_out frame id is given. -### required parameters +### Required parameters * `world_frame` (ℛw): frame in which the `CoG.force` is represented. * `sensor_frame` (ℛs): frame in which the `CoG.pos` is defined * `CoG.pos` (ps): position of the CoG of the mass the filter should compensate for * `CoG.force` (gw): constant (but updatable) force of gravity at the Cog (typically m.G), defined along axes of the `world_frame` -### algorithm +### Algorithm Given * above-required parameters, ℛw, ℛs, ps, gw -* `data_in`, a wrench ℱi = {fi, τi} represented in `data_in.frame` ℛi -* access to TF homogeneous transforms: +* `data_in`, a wrench ℱi = {fi, τi} represented in the `data_in` frame ℛi +* access to tf2 homogeneous transforms: * Tsi from ℛi to ℛs * Tsw from ℛw to ℛs * Tos from ℛs to ℛo -Compute `data_out` compensated wrench ℱco = {fco, τco} represented in `data_out.frame` ℛo if given, or `data_in.frame` ℛi otherwise, with equations: +Compute `data_out` compensated wrench ℱco = {fco, τco} represented in the `data_out` frame ℛo if given, or the `data_in` frame ℛi otherwise, with equations: ℱco = Tos.ℱcs, @@ -40,20 +38,20 @@ with ℱcs = {fcs, τcs} the compensated and, -fcs = fs - Tsw.gw +fcs = fs - Tswgw its force and, -τcs = τs - ps x (Tsw.gw) +τcs = τs - ps x (Tswgw) its torque, and -ℱs = Tsi.ℱi +ℱs = Tsi.ℱi = {fs, τs} the full transform of the input wrench ℱi to sensor frame ℛs Remarks : * a full vector is used for gravity force, to not impose gravity to be only along z of `world_frame`. -* `data_in.frame` is usually equal to `sensor_frame`, but could be different since measurement of wrech might occur in another frame. Ex: measurements are at the **FT sensor flange** = `data_in.frame`, but CoG is given in **FT sensor base** = `sensor_frame` (=frame to which it is mounted on the robot), introducing an offset (thichkess of the sensor) to be accounted for. -* `data_out.frame` is usually `data_in.frame`, but for convenience, can be set to any other useful frame. Ex: Wrench expressed in a `control_frame` for instance center of a gripper. +* `data_in` frame is usually equal to `sensor_frame`, but could be different since measurement of wrench might occur in another frame. E.g.: measurements are at the **FT sensor flange** = `data_in` frame, but CoG is given in **FT sensor base** = `sensor_frame` (=frame to which it is mounted on the robot), introducing an offset (thickness of the sensor) to be accounted for. +* `data_out` frame is usually `data_in` frame, but for convenience, can be set to any other useful frame. E.g.: wrench expressed in a `control_frame` like the center of a gripper. * Tsw will only rotate the gw vector, because gravity is a field applied everywhere, and not a wrench (no torque should be induced by transforming from ℛw to ℛs). diff --git a/src/control_filters/gravity_compensation_filter_parameters.yaml b/src/control_filters/gravity_compensation_filter_parameters.yaml index 95cfe8b0..ebfef4a3 100644 --- a/src/control_filters/gravity_compensation_filter_parameters.yaml +++ b/src/control_filters/gravity_compensation_filter_parameters.yaml @@ -1,8 +1,7 @@ gravity_compensation_filter: world_frame: { type: string, - # default_value: world, - description: "fixed world frame in which the constant field-induced force is given", + description: "Fixed world frame in which the constant field-induced force is given.", read_only: true, validation: { not_empty<>: [] @@ -10,8 +9,7 @@ gravity_compensation_filter: } sensor_frame: { type: string, - # default_value: ft_sensor, - description: "Sensor frame in which center of gravity (CoG) is measured and computation occur", + description: "Sensor frame in which center of gravity (CoG) is measured and computation occur.", read_only: true, validation: { not_empty<>: [] From 819941c86608b2ff81fdf14b404beceb96896c01 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Tue, 9 May 2023 11:32:44 +0200 Subject: [PATCH 24/25] Cleaned up TODO --- test/control_filters/test_gravity_compensation.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index 39ac1055..b1e9e08a 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -99,8 +99,6 @@ TEST_F(GravityCompensationTest, TestGravityCompensationComputation) out.header.frame_id = "base"; // should fail due to missing transform for desired output frame ASSERT_FALSE(filter_->update(in, out)); - - // TODO(guihomework) Add a test with real lookups } int main(int argc, char ** argv) From 1c960dd37272a5aad9c350a2223c11d261c0f927 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 10 May 2023 10:22:38 +0200 Subject: [PATCH 25/25] Fixed build fail in symlink mode --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 39531a87..af242007 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,6 +131,7 @@ install(TARGETS control_toolbox RUNTIME DESTINATION bin) install(TARGETS gravity_compensation gravity_compensation_filter_parameters + EXPORT export_gravity_compensation ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin