diff --git a/CMakeLists.txt b/CMakeLists.txt index 44f4ed5e..7a0be99f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,44 @@ ament_target_dependencies(${PROJECT_NAME} rcutils realtime_tools) +######################## +# Control filters +######################## +find_package(filters REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(Eigen3 REQUIRED) + +set(CONTROL_FILTERS_INCLUDE_DEPENDS + Eigen3 + filters + geometry_msgs + pluginlib + rclcpp + tf2_geometry_msgs + tf2_ros +) + +add_library(gravity_compensation SHARED + src/control_filters/gravity_compensation.cpp +) +target_include_directories(gravity_compensation PUBLIC include) +target_link_libraries(gravity_compensation parameter_handler) +ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + +add_library(low_pass_filter SHARED + src/control_filters/low_pass_filter.cpp +) +target_include_directories(low_pass_filter PUBLIC include) +target_link_libraries(low_pass_filter parameter_handler) +ament_target_dependencies(low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + +########## +# Testing +########## if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) @@ -53,6 +91,15 @@ 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}) + + ament_add_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp) + target_link_libraries(test_low_pass_filter low_pass_filter) + ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() # Install @@ -64,11 +111,28 @@ install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +install(TARGETS gravity_compensation low_pass_filter + 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 + low_pass_filter +) 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..46fc8688 --- /dev/null +++ b/control_filters.xml @@ -0,0 +1,29 @@ + + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + + + + + + This is a low pass filter working with a double value. + + + + + This is a low pass filter working with geometry_msgs::WrenchStamped. + + + + diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp new file mode 100644 index 00000000..0292599e --- /dev/null +++ b/include/control_filters/gravity_compensation.hpp @@ -0,0 +1,171 @@ +// 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 +#include +#include + +#include "control_toolbox/parameter_handler.hpp" +#include "filters/filter_base.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +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 +{ +public: + /** \brief Constructor */ + GravityCompensation(); + + /** \brief Destructor */ + ~GravityCompensation(); + + /** @brief Configure filter parameters */ + bool configure() override; + + /** \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 + */ + bool update(const T & data_in, T & data_out) override; + +private: + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr logger_; + std::unique_ptr parameters_; + + // 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_; +}; + +template +GravityCompensation::GravityCompensation() +{ +} + +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)); + + 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; + } + + // 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 control_filters + +#endif // CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp new file mode 100644 index 00000000..cf99fcac --- /dev/null +++ b/include/control_filters/low_pass_filter.hpp @@ -0,0 +1,226 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ + +#include +#include +#include +#include +#include + +#include "control_toolbox/parameter_handler.hpp" +#include "filters/filter_base.hpp" + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace control_filters +{ +class LowPassParameters : public control_toolbox::ParameterHandler +{ +public: + explicit LowPassParameters(const std::string & params_prefix) + : control_toolbox::ParameterHandler(params_prefix, 0, 1, 3) + { + add_double_parameter("sampling_frequency", false); + add_double_parameter("damping_frequency", false); + add_double_parameter("damping_intensity", false); + + add_integer_parameter("divider", false); + } + + bool check_if_parameters_are_valid() override + { + bool ret = true; + + // Check if any string parameter is empty + ret = !empty_parameter_in_list(string_parameters_); + + for (size_t i = 0; i < 3; ++i) + { + if (std::isnan(double_parameters_[i].second)) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Parameter '%s' has to be set", + double_parameters_[i].first.name.c_str()); + ret = false; + } + } + + if (integer_parameters_[0].second < 0) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Parameter '%s' has to be positive", + integer_parameters_[0].first.name.c_str()); + } + + return ret; + } + + void update_storage() override + { + sampling_frequency_ = double_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Sampling frequency is %e", sampling_frequency_); + damping_frequency_ = double_parameters_[1].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Damping frequency is %e", damping_frequency_); + damping_intensity_ = double_parameters_[2].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Damping intensity is %e", damping_intensity_); + + divider_ = integer_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Divider %d", divider_); + + a1_ = exp( + -1.0 / sampling_frequency_ * (2.0 * M_PI * damping_frequency_) / + (pow(10.0, damping_intensity_ / -10.0))); + b1_ = 1.0 - a1_; + } + + // Parameters from parameter server + double sampling_frequency_; + double damping_frequency_; + double damping_intensity_; + + int divider_; + + // Filter Parameters + double a1_; + double b1_; +}; + +template +class LowPassFilter : public filters::FilterBase +{ +public: + LowPassFilter(); + + ~LowPassFilter() override; + + bool configure() override; + + bool update(const T & data_in, T & data_out) override; + +private: + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr logger_; + std::unique_ptr parameters_; + + // Callback for updating dynamic parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; + + // Filter parameters + // TODO(destogl): we should do this more intelligently using only one set of types + double filtered_value, filtered_old_value, old_value; + Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; +}; + +template +LowPassFilter::LowPassFilter() +{ +} + +template +LowPassFilter::~LowPassFilter() +{ +} + +template +bool LowPassFilter::configure() +{ + clock_ = std::make_shared(RCL_SYSTEM_TIME); + logger_.reset( + new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); + parameters_.reset(new LowPassParameters(this->param_prefix_)); + + parameters_->initialize(this->params_interface_, logger_->get_name()); + + parameters_->declare_parameters(); + + if (!parameters_->get_parameters()) + { + return false; + } + + // Initialize storage Vectors + filtered_value = filtered_old_value = old_value = 0; + // TODO(destogl): make the size parameterizable and more intelligent is using complex types + for (size_t i = 0; i < 6; ++i) + { + msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; + } + + // 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; +} + +template <> +inline bool LowPassFilter::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(); + + // IIR Filter + msg_filtered = parameters_->b1_ * msg_old + parameters_->a1_ * msg_filtered_old; + msg_filtered_old = msg_filtered; + + // TODO(destogl): use wrenchMsgToEigen + msg_old[0] = data_in.wrench.force.x; + msg_old[1] = data_in.wrench.force.y; + msg_old[2] = data_in.wrench.force.z; + msg_old[3] = data_in.wrench.torque.x; + msg_old[4] = data_in.wrench.torque.y; + msg_old[5] = data_in.wrench.torque.z; + + data_out.wrench.force.x = msg_filtered[0]; + data_out.wrench.force.y = msg_filtered[1]; + data_out.wrench.force.z = msg_filtered[2]; + data_out.wrench.torque.x = msg_filtered[3]; + data_out.wrench.torque.y = msg_filtered[4]; + data_out.wrench.torque.z = msg_filtered[5]; + return true; +} + +template +bool LowPassFilter::update(const T & data_in, T & data_out) +{ + if (!this->configured_) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + return false; + } + + parameters_->update(); + + // Filter + data_out = parameters_->b1_ * old_value + parameters_->a1_ * filtered_old_value; + filtered_old_value = data_out; + old_value = data_in; + + return true; +} + +} // namespace control_filters + +#endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ diff --git a/package.xml b/package.xml index 8d8b13a7..90cf11aa 100644 --- a/package.xml +++ b/package.xml @@ -22,9 +22,14 @@ control_msgs + filters + geometry_msgs + pluginlib rclcpp rcutils realtime_tools + tf2_geometry_msgs + ament_cmake_gmock ament_cmake_gtest diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp new file mode 100644 index 00000000..3980ae18 --- /dev/null +++ b/src/control_filters/gravity_compensation.cpp @@ -0,0 +1,87 @@ +// 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 "geometry_msgs/msg/vector3_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.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, + filters::FilterBase) diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp new file mode 100644 index 00000000..ccd5fe2b --- /dev/null +++ b/src/control_filters/low_pass_filter.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "control_filters/low_pass_filter.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, filters::FilterBase) + +PLUGINLIB_EXPORT_CLASS( + control_filters::LowPassFilter, + 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..39980a72 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.hpp @@ -0,0 +1,65 @@ +// 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__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 +{ +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_; +}; + +#endif // CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ 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")); +// } diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp new file mode 100644 index 00000000..8be57b2c --- /dev/null +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -0,0 +1,57 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadLowPassFilter, load_low_pass_filter) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_low_pass_filter", "controlFilters/LowPassFilter")); + + rclcpp::shutdown(); +} + +TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_low_pass_filter", "controlFilters/LowPassFilterWrench")); + + rclcpp::shutdown(); +} diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp new file mode 100644 index 00000000..5b6c517e --- /dev/null +++ b/test/control_filters/test_low_pass_filter.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_low_pass_filter.hpp" + +TEST_F(LowPassFilterTest, TestLowPassFilter) +{ + double sampling_freq = 1000.0; + double damping_freq = 20.5; + double damping_intensity = 1.25; + int divider = 3; + + bool a1, b1; + a1 = exp(-1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0))); + b1 = 1.0 - a1; + + geometry_msgs::msg::WrenchStamped in, calculated, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + + //not yet configured, should deny update + ASSERT_FALSE(low_pass_filter_.update(in,out)); + + //should deny configuration as parameters are not yet set + ASSERT_FALSE(low_pass_filter_.configure()); + + //declare parameters and configure + node_->declare_parameter("sampling_frequency", sampling_freq); + node_->declare_parameter("damping_frequency", damping_freq); + node_->declare_parameter("damping_intensity", damping_intensity); + node_->declare_parameter("divider", divider); + ASSERT_TRUE(low_pass_filter_.configure()); + + + //first filter pass, output should be zero as no old wrench was stored + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, 0.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_EQ(out.wrench.force.z, 0.0); + ASSERT_EQ(out.wrench.torque.x, 0.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); + + //second filter pass with same values: + //calculate wrench by hand + calculated.wrench.force.x = b1 * in.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z; + //check equality with low-pass-filter + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); + + //update once again and check results + //calculate wrench by hand + calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * calculated.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * calculated.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * calculated.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * calculated.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * calculated.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * calculated.wrench.torque.z; + //check equality with low-pass-filter + ASSERT_TRUE(low_pass_filter_.update(in, out)); + ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); + +} diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp new file mode 100644 index 00000000..c2dd4e27 --- /dev/null +++ b/test/control_filters/test_low_pass_filter.hpp @@ -0,0 +1,62 @@ +// 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__TEST_LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ + +#include +#include +#include "gmock/gmock.h" + +#include "control_filters/low_pass_filter.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_low_pass_filter"); +} // namespace + +class LowPassFilterTest : public ::testing::Test +{ +public: + void SetUp() override + { + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + LowPassFilterTest() + : node_(std::make_shared("test_low_pass_filter")), + executor_(std::make_shared()) + { + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + control_filters::LowPassFilter low_pass_filter_; + std::thread executor_thread_; +}; + +#endif // CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_