diff --git a/CMakeLists.txt b/CMakeLists.txt index e8043922..942d4b05 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,38 @@ ament_target_dependencies(${PROJECT_NAME} rcutils realtime_tools) +######################## +# Build control filters +######################## +find_package(filters REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +set(CONTROL_FILTERS_INCLUDE_DEPENDS + filters + geometry_msgs + pluginlib + rclcpp + tf2_geometry_msgs + tf2_ros +) + +add_library(gravity_compensation SHARED + src/control_filters/gravity_compensation.cpp +) +target_include_directories(gravity_compensation PUBLIC + $ + $ +) +ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + +########## +# Testing +########## if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) @@ -57,6 +89,12 @@ if(BUILD_TESTING) ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) target_link_libraries(pid_publisher_tests ${PROJECT_NAME}) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) + + ## Control Filters + ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) + target_link_libraries(test_gravity_compensation gravity_compensation) + ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + endif() # Install @@ -68,11 +106,27 @@ install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +install(TARGETS gravity_compensation + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install pluginlib xml +pluginlib_export_plugin_description_file(filters control_filters.xml) + ament_export_dependencies( rclcpp rcutils - realtime_tools) -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) + realtime_tools + ${CONTROL_FILTERS_INCLUDE_DEPENDS} +) +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} + gravity_compensation +) ament_package() diff --git a/README.md b/README.md index 21acbbc7..369c15c5 100644 --- a/README.md +++ b/README.md @@ -27,3 +27,25 @@ doi = {10.21105/joss.00456}, URL = {http://www.theoj.org/joss-papers/joss.00456/10.21105.joss.00456.pdf} } ``` + + +## Code Formatting + +This repository uses `pre-commit` tool for code formatting. +The tool checks formatting each time you commit to a repository. +To install it locally use: + ``` + pip3 install pre-commit # (prepend `sudo` if you want to install it system wide) + ``` + +To run it initially over the whole repo you can use: + ``` + pre-commit run -a + ``` + +If you get error that something is missing on your computer, do the following for: + + - `clang-format-10` + ``` + sudo apt install clang-format-10 + ``` diff --git a/control_filters.xml b/control_filters.xml new file mode 100644 index 00000000..ca9f535b --- /dev/null +++ b/control_filters.xml @@ -0,0 +1,13 @@ + + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + + + diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp new file mode 100644 index 00000000..7e4d7fcd --- /dev/null +++ b/include/control_filters/gravity_compensation.hpp @@ -0,0 +1,222 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP +#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/LinearMath/Transform.h" +#include "filters/filter_base.hpp" + +namespace control_filters +{ + template + class GravityCompensation : public filters::FilterBase + { + public: + /** \brief Constructor */ + GravityCompensation(); + + /** \brief Destructor */ + ~GravityCompensation(); + + /** @brief Configure filter parameters */ + virtual bool configure() override; + + /** \brief Update the filter and return the data seperately + * \param data_in T array with length width + * \param data_out T array with length width + */ + virtual bool update(const T& data_in, T& data_out) override; + + /** \brief Get most recent parameters */ + bool updateParameters(); + + private: + /** \brief Dynamic parameter callback activated when parameters change */ + // void parameterCallback(); + + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + // Storage for Calibration Values + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) + double force_z_; // Gravitational Force + + // Frames for Transformation of Gravity / CoG Vector + std::string world_frame_; + std::string sensor_frame_; + std::string force_frame_; + + // tf2 objects + std::unique_ptr p_tf_Buffer_; + std::unique_ptr p_tf_Listener_; + geometry_msgs::msg::TransformStamped transform_, transform_back_, transform_cog_; + + bool configured_; + + uint num_transform_errors_; + }; + + template + GravityCompensation::GravityCompensation() + : logger_(rclcpp::get_logger("GravityCompensation")) + , configured_(false) + , num_transform_errors_(0) + { + } + + template + GravityCompensation::~GravityCompensation() + { + } + + template + bool GravityCompensation::configure() + { + clock_ = std::make_shared(RCL_SYSTEM_TIME); + p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); + p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); + + if(!updateParameters()){ + return false; + } + else{ + configured_ = true; + } + RCLCPP_INFO(logger_, + "Gravity Compensation Params: world frame: %s; sensor frame: %s; force frame: %s; CoG x:%f; " + "CoG y:%f; CoG z:%f; force: %f.", + world_frame_.c_str(), sensor_frame_.c_str(), force_frame_.c_str(), cog_.vector.x, cog_.vector.y, cog_.vector.z, force_z_); + + return true; + } + + template + bool GravityCompensation::update(const T& data_in, T& data_out) + { + if (!configured_) + { + RCLCPP_ERROR(logger_, "Filter is not configured"); + return false; + } + + //if (params_updated_) + //{ + // updateParameters(); + //} + + try + { + transform_ = p_tf_Buffer_->lookupTransform(world_frame_, data_in.header.frame_id, rclcpp::Time()); + transform_back_ = p_tf_Buffer_->lookupTransform(data_in.header.frame_id, world_frame_, rclcpp::Time()); + transform_cog_ = p_tf_Buffer_->lookupTransform(world_frame_, force_frame_, rclcpp::Time()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", ex.what()); + } + + geometry_msgs::msg::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; + + temp_vector_in.vector = data_in.wrench.force; + tf2::doTransform(temp_vector_in, temp_force_transformed, transform_); + + temp_vector_in.vector = data_in.wrench.torque; + tf2::doTransform(temp_vector_in, temp_torque_transformed, transform_); + + // Transform CoG Vector + geometry_msgs::msg::Vector3Stamped cog_transformed; + tf2::doTransform(cog_, cog_transformed, transform_cog_); + + // Compensate for gravity force + temp_force_transformed.vector.z += force_z_; + // Compensation Values for torque result from Crossprod of cog Vector and (0 0 G) + temp_torque_transformed.vector.x += (force_z_ * cog_transformed.vector.y); + temp_torque_transformed.vector.y -= (force_z_ * cog_transformed.vector.x); + + // Copy Message and Compensate values for Gravity Force and Resulting Torque + // geometry_msgs::WrenchStamped compensated_wrench; + data_out = data_in; + + tf2::doTransform(temp_force_transformed, temp_vector_out, transform_back_); + data_out.wrench.force = temp_vector_out.vector; + + tf2::doTransform(temp_torque_transformed, temp_vector_out, transform_back_); + data_out.wrench.torque = temp_vector_out.vector; + + return true; + } + + template + bool GravityCompensation::updateParameters() + { + //params_updated_ = false; + + if (!filters::FilterBase::getParam("world_frame", world_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param world_frame_"); + return false; + } + if (!filters::FilterBase::getParam("sensor_frame", sensor_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param sensor_frame"); + return false; + } + if (!filters::FilterBase::getParam("force_frame", force_frame_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param force_frame"); + return false; + } + if (!filters::FilterBase::getParam("CoG_x", cog_.vector.x)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_x"); + return false; + } + if (!filters::FilterBase::getParam("CoG_y", cog_.vector.y)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_y"); + return false; + } + if (!filters::FilterBase::getParam("CoG_z", cog_.vector.z)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param CoG_z"); + return false; + } + if (!filters::FilterBase::getParam("force", force_z_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Gravitiy Compensator did not find param force"); + return false; + } + return true; + } + + //template + //void GravityCompensation::parameterCallback() + //{ + // params_updated_ = true; + //} + +} // namespace iirob_filters +#endif diff --git a/package.xml b/package.xml index 2ee37d08..8f9f4665 100644 --- a/package.xml +++ b/package.xml @@ -22,9 +22,14 @@ control_msgs + filters + geometry_msgs + pluginlib rclcpp rcutils realtime_tools + tf2_geometry_msgs + ament_cmake_gmock ament_cmake_gtest diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp new file mode 100644 index 00000000..6c38439f --- /dev/null +++ b/src/control_filters/gravity_compensation.cpp @@ -0,0 +1,20 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "control_filters/gravity_compensation.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::GravityCompensation, + filters::FilterBase) diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp new file mode 100644 index 00000000..ae9d5f05 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_gravity_compensation.hpp" + +TEST_F(GravityCompensationTest, TestGravityCompensation) +{ + node_->declare_parameter("world_frame", "world"); + node_->declare_parameter("sensor_frame", "sensor"); + node_->declare_parameter("CoG_x", 0.0); + node_->declare_parameter("CoG_y", 0.0); + node_->declare_parameter("CoG_z", 0.0); + node_->declare_parameter("force", 50.0); + + ASSERT_TRUE(gravity_compensation_.configure()); + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + ASSERT_TRUE(gravity_compensation_.update(in, out)); + + ASSERT_EQ(out.wrench.force.x, 1.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_EQ(out.wrench.force.z, 50.0); + + ASSERT_EQ(out.wrench.torque.x, 10.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); +} diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp new file mode 100644 index 00000000..a614ebb6 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gmock/gmock.h" +#include + +#include "control_filters/gravity_compensation.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_gravity_compensation"); +} // namespace + +// TODO(destogl): do this +// subclassing and friending so we can access member variables + + +class GravityCompensationTest : public ::testing::Test +{ +public: + void SetUp() override + { + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + GravityCompensationTest() + : node_(std::make_shared("test_gravity_compensation")) + , executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + control_filters::GravityCompensation gravity_compensation_; + std::thread executor_thread_; +}; diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp new file mode 100644 index 00000000..22f66c1e --- /dev/null +++ b/test/control_filters/test_load_gravity_compensation.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// #include +// +// #include +// +// #include "controller_manager/controller_manager.hpp" +// #include "hardware_interface/resource_manager.hpp" +// #include "rclcpp/executor.hpp" +// #include "rclcpp/executors/single_threaded_executor.hpp" +// #include "rclcpp/utilities.hpp" +// #include "ros2_control_test_assets/descriptions.hpp" +// +// TEST(TestLoadAdmittanceController, load_controller) +// { +// rclcpp::init(0, nullptr); +// +// std::shared_ptr executor = +// std::make_shared(); +// +// controller_manager::ControllerManager cm(std::make_unique( +// ros2_control_test_assets::minimal_robot_urdf), +// executor, "test_controller_manager"); +// +// ASSERT_NO_THROW( +// cm.load_controller( +// "test_admittance_controller", +// "admittance_controller/AdmittanceController")); +// }