removed saturateTorqueRate from combinable hw, set default cutoff freq to 1000

franka_control/config/franka_combined_control_node.yaml

@@ -19,7 +19,7 @@ panda_1:
   # Activate rate limiter? [true|false]
   rate_limiting: true
   # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
-  cutoff_frequency: 100
+  cutoff_frequency: 1000
   # Internal controller for motion generators [joint_impedance|cartesian_impedance]
   internal_controller: joint_impedance
   # Used to decide whether to enforce realtime mode [enforce|ignore]
@@ -52,7 +52,7 @@ panda_2:
   # Activate rate limiter? [true|false]
   rate_limiting: true
   # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
-  cutoff_frequency: 100
+  cutoff_frequency: 1000
   # Internal controller for motion generators [joint_impedance|cartesian_impedance]
   internal_controller: joint_impedance
   # Used to decide whether to enforce realtime mode [enforce|ignore]

franka_hw/include/franka_hw/franka_combinable_hw.h

@@ -156,9 +156,6 @@ class FrankaCombinableHW : public FrankaHW {
 
   void controlLoop();
 
-  static std::array<double, 7> saturateTorqueRate(const std::array<double, 7>& tau_d_calculated,
-                                                  const std::array<double, 7>& tau_J_d);
-
   std::unique_ptr<std::thread> control_loop_thread_;
   ServiceContainer services_;
   std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>

franka_hw/src/franka_combinable_hw.cpp

@@ -158,18 +158,6 @@ void FrankaCombinableHW::write(const ros::Time& time, const ros::Duration& perio
   FrankaHW::write(time, period);
 }
 
-std::array<double, 7> FrankaCombinableHW::saturateTorqueRate(
-    const std::array<double, 7>& tau_d_calculated,
-    const std::array<double, 7>& tau_J_d) {  // NOLINT (readability-identifier-naming)
-  const double kDeltaTauMax = 1.0;
-  std::array<double, 7> tau_d_saturated{};
-  for (size_t i = 0; i < 7; i++) {
-    double difference = tau_d_calculated[i] - tau_J_d[i];
-    tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
-  }
-  return tau_d_saturated;
-}
-
 std::string FrankaCombinableHW::getArmID() const noexcept {
   return arm_id_;
 }