Add gravity compensation filter

control_toolbox/CMakeLists.txt

@@ -20,6 +20,7 @@ endif()
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   control_msgs
   rclcpp
+  rcl_interfaces
   rcutils
   realtime_tools
 )
@@ -51,16 +52,36 @@ target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIB
 ########################
 set(CONTROL_FILTERS_INCLUDE_DEPENDS
   filters
-  rclcpp
+  geometry_msgs
   generate_parameter_library
   pluginlib
-  geometry_msgs
+  rclcpp
+  tf2_geometry_msgs
+  tf2
+  tf2_ros
 )
+
 foreach(Dependency IN ITEMS ${CONTROL_FILTERS_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 find_package(Eigen3 REQUIRED NO_MODULE)
 
+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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/control_toolbox>
+)
+target_link_libraries(gravity_compensation PUBLIC gravity_compensation_filter_parameters)
+ament_target_dependencies(gravity_compensation PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+
 generate_parameter_library(
   low_pass_filter_parameters
   src/control_filters/low_pass_filter_parameters.yaml
@@ -144,6 +165,17 @@ if(BUILD_TESTING)
   ament_target_dependencies(pid_ros_publisher_tests rclcpp_lifecycle)
 
   ## Control Filters
+  ### Gravity Compensation
+  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})
+
+  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})
+
   # exponential_filter
   add_rostest_with_parameters_gmock(test_exponential_filter test/control_filters/test_exponential_filter.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_exponential_filter_parameters.yaml
@@ -189,7 +221,7 @@ install(
 install(TARGETS control_toolbox
   low_pass_filter low_pass_filter_parameters
   rate_limiter rate_limiter_parameters
-  exponential_filter exponential_filter_parameters
+  exponential_filter exponential_filter_parameters gravity_compensation gravity_compensation_filter_parameters
   EXPORT export_control_toolbox
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

control_toolbox/control_filters.xml

@@ -1,4 +1,15 @@
 <class_libraries>
+    <library path="gravity_compensation">
+      <class name="control_filters/GravityCompensationWrench"
+            type="control_filters::GravityCompensation&lt;geometry_msgs::msg::WrenchStamped&gt;"
+            base_class_type="filters::FilterBase&lt;geometry_msgs::msg::WrenchStamped&gt;">
+        <description>
+          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.
+        </description>
+      </class>
+    </library>
     <library path="low_pass_filter">
       <class name="control_filters/LowPassFilterDouble"
              type="control_filters::LowPassFilter&lt;double&gt;"

control_toolbox/include/control_filters/gravity_compensation.hpp

@@ -0,0 +1,127 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_
+#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_
+
+#include <exception>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "filters/filter_base.hpp"
+#include "geometry_msgs/msg/vector3_stamped.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "control_toolbox/gravity_compensation_filter_parameters.hpp"
+
+namespace control_filters
+{
+
+
+template <typename T>
+class GravityCompensation : public filters::FilterBase<T>
+{
+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;
+
+protected:
+  void compute_internal_params()
+  {
+    cog_.vector.x = parameters_.tool.CoG[0];
+    cog_.vector.y = parameters_.tool.CoG[1];
+    cog_.vector.z = parameters_.tool.CoG[2];
+    cst_ext_force_.vector.x = parameters_.tool.gravity_field[0]*parameters_.tool.mass;
+    cst_ext_force_.vector.y = parameters_.tool.gravity_field[1]*parameters_.tool.mass;
+    cst_ext_force_.vector.z = parameters_.tool.gravity_field[2]*parameters_.tool.mass;
+  };
+
+private:
+  rclcpp::Clock::SharedPtr clock_;
+  std::shared_ptr<rclcpp::Logger> logger_;
+  std::shared_ptr<gravity_compensation_filter::ParamListener> parameter_handler_;
+  gravity_compensation_filter::Params parameters_;
+
+  // Frames for Transformation of Gravity / CoG Vector
+  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 cst_ext_force_;  // Gravity Force Vector (wrt world frame)
+
+  // Filter objects
+  std::unique_ptr<tf2_ros::Buffer> p_tf_Buffer_;
+  std::unique_ptr<tf2_ros::TransformListener> p_tf_Listener_;
+  geometry_msgs::msg::TransformStamped transform_sensor_datain_, transform_world_dataout_,
+    transform_data_out_sensor_, transform_sensor_world_;
+};
+
+template <typename T>
+GravityCompensation<T>::GravityCompensation()
+{
+}
+
+template <typename T>
+GravityCompensation<T>::~GravityCompensation()
+{
+}
+
+template <typename T>
+bool GravityCompensation<T>::configure()
+{
+  clock_ = std::make_shared<rclcpp::Clock>(RCL_ROS_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_)));
+
+  // Initialize the parameter_listener once
+  if (!parameter_handler_)
+  {
+    try
+    {
+      parameter_handler_ =
+        std::make_shared<gravity_compensation_filter::ParamListener>(this->params_interface_,
+                                                                     this->param_prefix_);
+    }
+    catch (std::exception & ex) {
+      RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what());
+      parameter_handler_.reset();
+      throw;
+    }
+  }
+  parameters_ = parameter_handler_->get_params();
+  compute_internal_params();
+
+  return true;
+}
+
+}  // namespace control_filters
+
+#endif  // CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_

control_toolbox/package.xml

@@ -32,6 +32,9 @@
   <depend>rclcpp</depend>
   <depend>rcutils</depend>
   <depend>realtime_tools</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>rclcpp_lifecycle</test_depend>

control_toolbox/src/control_filters/README.md

@@ -4,9 +4,65 @@ Implement filter plugins for control purposes as https://index.ros.org/r/filters
 
 ## Available filters
 
+* Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench).
 * Low Pass: implements a low-pass filter based on a time-invariant [Infinite Impulse Response (IIR) filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), for different data types (doubles or wrench).
 * Exponential Filter: Exponential filter for double data type.
 
+
+## Gravity compensation filter
+
+This filter implements an algorithm compensating for the gravity forces acting at the center of gravity (CoG) of a known mass, computed at a `sensor_frame` and applied to a `data_in` wrench.
+
+ 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
+
+* `world_frame` (&Rscr;<sub>w</sub>): frame in which the `CoG.force` is represented.
+* `sensor_frame` (&Rscr;<sub>s</sub>): frame in which the `CoG.pos` is defined
+* `CoG.pos` (p<sub>s</sub>): position of the CoG of the mass the filter should compensate for
+* `CoG.force` (g<sub>w</sub>): constant (but updatable) force of gravity at the Cog (typically m.G), defined along axes of the `world_frame`
+
+### Algorithm
+
+Given
+
+* above-required parameters,  &Rscr;<sub>w</sub>, &Rscr;<sub>s</sub>, p<sub>s</sub>, g<sub>w</sub>
+* `data_in`, a wrench &Fscr;<sub>i</sub> = {f<sub>i</sub>, &tau;<sub>i</sub>} represented in the `data_in` frame &Rscr;<sub>i</sub>
+* access to tf2 homogeneous transforms:
+  * T<sub>si</sub> from &Rscr;<sub>i</sub> to &Rscr;<sub>s</sub>
+  * T<sub>sw</sub> from &Rscr;<sub>w</sub> to &Rscr;<sub>s</sub>
+  * T<sub>os</sub> from &Rscr;<sub>s</sub> to &Rscr;<sub>o</sub>
+
+Compute `data_out` compensated wrench &Fscr;c<sub>o</sub> = {fc<sub>o</sub>, &tau;c<sub>o</sub>} represented in the `data_out` frame &Rscr;<sub>o</sub> if given, or the `data_in` frame &Rscr;<sub>i</sub> otherwise, with equations:
+
+&Fscr;c<sub>o</sub> = T<sub>os</sub>.&Fscr;c<sub>s</sub>,
+
+
+with &Fscr;c<sub>s</sub> = {fc<sub>s</sub>, &tau;c<sub>s</sub>}  the compensated wrench in `sensor_frame` (common frame for computation)
+
+and,
+
+fc<sub>s</sub> = f<sub>s</sub> - T<sub>sw</sub>g<sub>w</sub>
+
+its force  and,
+
+&tau;c<sub>s</sub> = &tau;<sub>s</sub> - p<sub>s</sub> x (T<sub>sw</sub>g<sub>w</sub>)
+
+its torque, and
+
+&Fscr;<sub>s</sub>  = T<sub>si</sub>.&Fscr;<sub>i</sub> = {f<sub>s</sub>, &tau;<sub>s</sub>}
+
+the full transform of the input wrench &Fscr;<sub>i</sub> to sensor frame &Rscr;<sub>s</sub>
+
+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 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.
+* T<sub>sw</sub> will only rotate the g<sub>w</sub> vector, because gravity is a field applied everywhere, and not a wrench (no torque should be induced by transforming from &Rscr;<sub>w</sub> to &Rscr;<sub>s</sub>).
+
+
 ## Low Pass filter
 
 This filter implements a low-pass filter in the form of an [IIR filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), applied to a `data_in` (double or wrench).
@@ -21,6 +77,7 @@ The feedforward and feedback coefficients of the IIR filter are computed from th
 ### Algorithm
 
 Given
+
 * above-required parameters,  `sf`, `df`, `di`
 * `data_in`, a double or wrench `x`