Removed gravity from tau_J_d and adjusted examples.

franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h

@@ -34,8 +34,7 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn
   // Saturation
   Eigen::Matrix<double, 7, 1> saturateTorqueRate(
       const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-      const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-      const Eigen::Matrix<double, 7, 1>& gravity);
+      const Eigen::Matrix<double, 7, 1>& tau_J_d);  // NOLINT (readability-identifier-naming)
 
   std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;

franka_example_controllers/include/franka_example_controllers/force_example_controller.h

@@ -33,8 +33,7 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro
   // Saturation
   Eigen::Matrix<double, 7, 1> saturateTorqueRate(
       const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-      const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-      const Eigen::Matrix<double, 7, 1>& gravity);
+      const Eigen::Matrix<double, 7, 1>& tau_J_d);  // NOLINT (readability-identifier-naming)
 
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
   std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;

franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h

@@ -33,8 +33,7 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf
   // Saturation
   std::array<double, 7> saturateTorqueRate(
       const std::array<double, 7>& tau_d_calculated,
-      const std::array<double, 7>& tau_J_d,  // NOLINT (readability-identifier-naming)
-      const std::array<double, 7>& gravity);
+      const std::array<double, 7>& tau_J_d);  // NOLINT (readability-identifier-naming)
 
   std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

@@ -186,7 +186,7 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
   // Desired torque
   tau_d << tau_task + tau_nullspace + coriolis;
   // Saturate torque rate to avoid discontinuities
-  tau_d << saturateTorqueRate(tau_d, tau_J_d, gravity);
+  tau_d << saturateTorqueRate(tau_d, tau_J_d);
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_d(i));
   }
@@ -211,14 +211,12 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
 
 Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
     const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-    const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-    const Eigen::Matrix<double, 7, 1>& gravity) {
+    const Eigen::Matrix<double, 7, 1>& tau_J_d) {  // NOLINT (readability-identifier-naming)
   Eigen::Matrix<double, 7, 1> tau_d_saturated{};
   for (size_t i = 0; i < 7; i++) {
-    // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
-    double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
+    double difference = tau_d_calculated[i] - tau_J_d[i];
     tau_d_saturated[i] =
-        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
+        tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
   }
   return tau_d_saturated;
 }

franka_example_controllers/src/force_example_controller.cpp

@@ -117,7 +117,7 @@ void ForceExampleController::update(const ros::Time& /*time*/, const ros::Durati
   tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext);
   // FF + PI control (PI gains are intially all 0)
   tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_;
-  tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d, gravity);
+  tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d);
 
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_cmd(i));
@@ -139,14 +139,11 @@ void ForceExampleController::desiredMassParamCallback(
 
 Eigen::Matrix<double, 7, 1> ForceExampleController::saturateTorqueRate(
     const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-    const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-    const Eigen::Matrix<double, 7, 1>& gravity) {
+    const Eigen::Matrix<double, 7, 1>& tau_J_d) {  // NOLINT (readability-identifier-naming)
   Eigen::Matrix<double, 7, 1> tau_d_saturated{};
   for (size_t i = 0; i < 7; i++) {
-    // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
-    double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
-    tau_d_saturated[i] =
-        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
+    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;
 }

franka_example_controllers/src/joint_impedance_example_controller.cpp

@@ -173,8 +173,7 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
 
   // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is
   // 1000 * (1 / sampling_time).
-  std::array<double, 7> tau_d_saturated =
-      saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d, gravity);
+  std::array<double, 7> tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d);
 
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_d_saturated[i]);
@@ -204,14 +203,11 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
 
 std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate(
     const std::array<double, 7>& tau_d_calculated,
-    const std::array<double, 7>& tau_J_d,  // NOLINT (readability-identifier-naming)
-    const std::array<double, 7>& gravity) {
+    const std::array<double, 7>& tau_J_d) {  // NOLINT (readability-identifier-naming)
   std::array<double, 7> tau_d_saturated{};
   for (size_t i = 0; i < 7; i++) {
-    // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
-    double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
-    tau_d_saturated[i] =
-        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
+    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;
 }