ADD: Configurable filter for tau_ext_hat_filtered in franka_gazebo

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
 

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

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_;

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());