From 9c0edfe02699c69ad47ab5e0d4d1289645b19b26 Mon Sep 17 00:00:00 2001
From: Toni Oliver <toni@shadowrobot.com>
Date: Mon, 13 Sep 2021 08:57:25 +0000
Subject: [PATCH 1/2] Fixing J0 (J1 and J2) position and velocity filter

---
 .../include/sr_robot_lib/sr_joint_motor.hpp   |  2 +
 sr_robot_lib/src/sr_motor_hand_lib.cpp        |  6 +++
 sr_robot_lib/src/sr_motor_robot_lib.cpp       | 39 +++++++++++++++----
 3 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/sr_robot_lib/include/sr_robot_lib/sr_joint_motor.hpp b/sr_robot_lib/include/sr_robot_lib/sr_joint_motor.hpp
index 98bfe733..cc21d26f 100644
--- a/sr_robot_lib/include/sr_robot_lib/sr_joint_motor.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/sr_joint_motor.hpp
@@ -186,6 +186,8 @@ struct Joint
   sr_math_utils::filters::LowPassFilter pos_filter;
   // used to filter the effort
   sr_math_utils::filters::LowPassFilter effort_filter;
+  // used to filter the position and the velocity of a second joint
+  sr_math_utils::filters::LowPassFilter pos_filter_2;
 
   bool has_actuator;
   boost::shared_ptr<SrActuatorWrapper> actuator_wrapper;
diff --git a/sr_robot_lib/src/sr_motor_hand_lib.cpp b/sr_robot_lib/src/sr_motor_hand_lib.cpp
index 768578a8..d4cc7c00 100644
--- a/sr_robot_lib/src/sr_motor_hand_lib.cpp
+++ b/sr_robot_lib/src/sr_motor_hand_lib.cpp
@@ -149,6 +149,12 @@ namespace shadow_robot
       joint.joint_name = joint_names[index];
       joint.joint_to_sensor = joint_to_sensors[index];
 
+      if ((!joint.joint_to_sensor.calibrate_after_combining_sensors)
+         && (joint.joint_to_sensor.joint_to_sensor_vector.size() == 2))
+      {
+        joint.pos_filter_2 = sr_math_utils::filters::LowPassFilter(tau);
+      }
+
       if (actuator_ids[index] != -1)
       {
         joint.has_actuator = true;
diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp
index 03ad56f6..c0f8e6ba 100644
--- a/sr_robot_lib/src/sr_motor_robot_lib.cpp
+++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp
@@ -1023,13 +1023,38 @@ namespace shadow_robot
     // calibrate the joint and update the position.
     calibrate_joint(joint_tmp, status_data);
 
-    // filter the position and velocity
-    pair<double, double> pos_and_velocity = joint_tmp->pos_filter.compute(actuator->motor_state_.position_unfiltered_,
-                                                                          timestamp);
-    // reset the position to the filtered value
-    actuator->state_.position_ = pos_and_velocity.first;
-    // set the velocity to the filtered velocity
-    actuator->state_.velocity_ = pos_and_velocity.second;
+    if ((!joint_tmp->joint_to_sensor.calibrate_after_combining_sensors)
+        && (joint_tmp->joint_to_sensor.joint_to_sensor_vector.size() == 2))
+    {
+      actuator->motor_state_.filtered_calibrated_position_values_.clear();
+      actuator->motor_state_.filtered_calibrated_velocity_values_.clear();
+
+      // filter the position and velocity
+      pair<double, double> pos_and_velocity = joint_tmp->pos_filter.compute(actuator->motor_state_.calibrated_sensor_values_[0],
+                                                                            timestamp);
+      // set the position to the filtered value
+      actuator->motor_state_.filtered_calibrated_position_values_.push_back(pos_and_velocity.first);
+      // set the velocity to the filtered velocity
+      actuator->motor_state_.filtered_calibrated_velocity_values_.push_back(pos_and_velocity.second);
+
+      // filter the position and velocity for the second joint
+      pos_and_velocity = joint_tmp->pos_filter_2.compute(actuator->motor_state_.calibrated_sensor_values_[1],
+                                                                            timestamp);
+      // set the position to the filtered value for the second joint
+      actuator->motor_state_.filtered_calibrated_position_values_.push_back(pos_and_velocity.first);
+      // set the velocity to the filtered velocity for the second joint
+      actuator->motor_state_.filtered_calibrated_velocity_values_.push_back(pos_and_velocity.second);
+    }
+    else
+    {
+      // filter the position and velocity
+      pair<double, double> pos_and_velocity = joint_tmp->pos_filter.compute(actuator->motor_state_.position_unfiltered_,
+                                                                            timestamp);
+      // reset the position to the filtered value
+      actuator->state_.position_ = pos_and_velocity.first;
+      // set the velocity to the filtered velocity
+      actuator->state_.velocity_ = pos_and_velocity.second;
+    }
   }
 
   template<class StatusType, class CommandType>

From b6a493ae2ff2d533906696d0dcfbe083f6f3f009 Mon Sep 17 00:00:00 2001
From: Toni Oliver <toni@shadowrobot.com>
Date: Wed, 11 May 2022 16:42:29 +0100
Subject: [PATCH 2/2] Read ROS param to set the tau for the effort filter

---
 sr_robot_lib/src/sr_motor_hand_lib.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/sr_robot_lib/src/sr_motor_hand_lib.cpp b/sr_robot_lib/src/sr_motor_hand_lib.cpp
index d4cc7c00..eacc25ec 100644
--- a/sr_robot_lib/src/sr_motor_hand_lib.cpp
+++ b/sr_robot_lib/src/sr_motor_hand_lib.cpp
@@ -155,6 +155,11 @@ namespace shadow_robot
         joint.pos_filter_2 = sr_math_utils::filters::LowPassFilter(tau);
       }
 
+      full_param << act_name << "/effort_filter/tau";
+      this->nodehandle_.template param<float>(full_param.str(), tau, 0.05);
+      full_param.str("");
+      joint.effort_filter = sr_math_utils::filters::LowPassFilter(tau);
+
       if (actuator_ids[index] != -1)
       {
         joint.has_actuator = true;