Add gravity compensation filter

CMakeLists.txt

@@ -16,6 +16,7 @@ endif()
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   control_msgs
   rclcpp
+  rcl_interfaces
   rcutils
   realtime_tools
 )
@@ -41,28 +42,44 @@ 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
 ########################
 set(CONTROL_FILTERS_INCLUDE_DEPENDS
+  Eigen3
   filters
-  rclcpp
+  geometry_msgs
   generate_parameter_library
   pluginlib
-  geometry_msgs
-  Eigen3
+  tf2_geometry_msgs
+  tf2
+  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
+    $<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
 )
-
 add_library(low_pass_filter SHARED
   src/control_filters/low_pass_filter.cpp
 )
@@ -75,6 +92,7 @@ target_include_directories(low_pass_filter PUBLIC
 target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters)
 ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS})
 
+
 # Install pluginlib xml
 pluginlib_export_plugin_description_file(filters control_filters.xml)
 
@@ -97,6 +115,18 @@ if(BUILD_TESTING)
   ament_target_dependencies(pid_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})
+
+  ### Low-pass
   add_rostest_with_parameters_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml
   )
@@ -113,7 +143,7 @@ install(
   DIRECTORY include/
   DESTINATION include/control_toolbox
 )
-install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters
+install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters gravity_compensation gravity_compensation_filter_parameters
   EXPORT export_control_toolbox
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

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-14`
+     ```
+     sudo apt install clang-format-14
+     ```

control_filters.xml

@@ -1,18 +1,29 @@
 <class_libraries>
-    <library path="low_pass_filter">
-      <class name="control_filters/LowPassFilterDouble"
-             type="control_filters::LowPassFilter&lt;double&gt;"
-             base_class_type="filters::FilterBase&lt;double&gt;">
-        <description>
-          This is a low pass filter working with a double value.
-        </description>
-      </class>
-      <class name="control_filters/LowPassFilterWrench"
-             type="control_filters::LowPassFilter&lt;geometry_msgs::msg::WrenchStamped&gt;"
-             base_class_type="filters::FilterBase&lt;geometry_msgs::msg::WrenchStamped&gt;">
-        <description>
-            This is a low pass filter working with geometry_msgs::WrenchStamped.
-        </description>
-      </class>
-    </library>
+  <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;"
+           base_class_type="filters::FilterBase&lt;double&gt;">
+      <description>
+        This is a low pass filter working with a double value.
+      </description>
+    </class>
+    <class name="control_filters/LowPassFilterWrench"
+           type="control_filters::LowPassFilter&lt;geometry_msgs::msg::WrenchStamped&gt;"
+           base_class_type="filters::FilterBase&lt;geometry_msgs::msg::WrenchStamped&gt;">
+      <description>
+          This is a low pass filter working with geometry_msgs::WrenchStamped.
+      </description>
+    </class>
+  </library>
 </class_libraries>

include/control_filters/gravity_compensation.hpp

@@ -0,0 +1,130 @@
+// 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 <memory>
+#include <string>
+#include <vector>
+
+#include "gravity_compensation_filter_parameters.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
+{
+
+
+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_.CoG.pos[0];
+    cog_.vector.y = parameters_.CoG.pos[1];
+    cog_.vector.z = parameters_.CoG.pos[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:
+  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_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_)));
+
+  // 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 (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();
+
+  return true;
+}
+
+}  // namespace control_filters
+
+#endif  // CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_

package.xml

@@ -26,6 +26,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>ament_cmake_gtest</test_depend>

src/control_filters/README.md

@@ -2,8 +2,64 @@
 
 ## 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).
 
+
+## 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 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).
@@ -18,6 +74,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`