From 553e18217ef3c7b702872c7f67450f832a3d7952 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Tue, 1 Feb 2022 11:20:00 +0100
Subject: [PATCH] ADD: Configurable filter for `tau_ext_hat_filtered` in
 franka_gazebo

---
 CHANGELOG.md                                        |  1 +
 franka_gazebo/config/franka_hw_sim.yaml             |  1 +
 franka_gazebo/include/franka_gazebo/franka_hw_sim.h |  4 ++++
 franka_gazebo/src/franka_hw_sim.cpp                 | 10 +++++++++-
 4 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index e9433a95d..e4770326d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -20,6 +20,7 @@
 * Improve Gazebo 'stone' world objects
 * Add `connected_to` option to `panda_gazebo.xacro` macro, similar to `panda_arm.xacro`
 * Rename `ns` -> `arm_id` in `hand.xacro` macros to be consistent with the other xacro files
+* Introduce new `tau_ext_lowpass_filter` parameter for `franka_gazebo` to configure the filtering of `tau_ext_hat_filtered`
 
 ## 0.8.1 - 2021-09-08
 
diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml
index fde8beaa5..6703a1682 100644
--- a/franka_gazebo/config/franka_hw_sim.yaml
+++ b/franka_gazebo/config/franka_hw_sim.yaml
@@ -1,5 +1,6 @@
 arm_id:                           $(arg arm_id)
 singularity_warning_threshold:    0.0001               # print a warning if the smallest singular value of J x J^T drops below this value (use -1 to disable)
+tau_ext_lowpass_filter:           1.0                  # Exponential Moving average filter: range between and zero (infinite delay) one (no filtering)
 
 franka_gripper:
   type:    franka_gazebo/FrankaGripperSim
diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h
index 671f4c78d..2cbd1fdd3 100644
--- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h
+++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h
@@ -20,6 +20,8 @@
 
 namespace franka_gazebo {
 
+const double kDefaultTauExtLowpassFilter = 1.0;  // no filtering per default of tau_ext_hat_filtered
+
 /**
  * A custom implementation of a [gazebo_ros_control](http://wiki.ros.org/gazebo_ros_control) plugin,
  * which is able to simulate franka interfaces in Gazebo.
@@ -107,6 +109,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   franka::RobotState robot_state_;
   std::unique_ptr<franka_hw::ModelBase> model_;
 
+  double tau_ext_lowpass_filter_;
+
   ros::ServiceServer service_set_ee_;
   ros::ServiceServer service_set_k_;
   ros::ServiceServer service_set_load_;
diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
index 9e6e98f48..dac452faa 100644
--- a/franka_gazebo/src/franka_hw_sim.cpp
+++ b/franka_gazebo/src/franka_hw_sim.cpp
@@ -47,6 +47,9 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
   auto gravity = physics->World()->Gravity();
   this->gravity_earth_ = {gravity.X(), gravity.Y(), gravity.Z()};
 
+  model_nh.param<double>("tau_ext_lowpass_filter", this->tau_ext_lowpass_filter_,
+                         kDefaultTauExtLowpassFilter);
+
   // Generate a list of franka_gazebo::Joint to store all relevant information
   for (const auto& transmission : transmissions) {
     if (transmission.type_ != "transmission_interface/SimpleTransmission") {
@@ -487,7 +490,12 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
     this->robot_state_.dtheta[i] = joint->velocity;
 
     if (this->efforts_initialized_) {
-      this->robot_state_.tau_ext_hat_filtered[i] = joint->effort - joint->command + joint->gravity;
+      double tau_ext = joint->effort - joint->command + joint->gravity;
+
+      // Exponential moving average filter from tau_ext -> tau_ext_hat_filtered
+      this->robot_state_.tau_ext_hat_filtered[i] =
+          this->tau_ext_lowpass_filter_ * tau_ext +
+          (1 - this->tau_ext_lowpass_filter_) * this->robot_state_.tau_ext_hat_filtered[i];
     }
 
     this->robot_state_.joint_contact[i] = static_cast<double>(joint->isInContact());