From 286feb62a75a2553e9ebc22f07bc774db4d2a9d2 Mon Sep 17 00:00:00 2001
From: rickstaa <rick.staa@outlook.com>
Date: Mon, 27 Sep 2021 13:04:09 +0200
Subject: [PATCH] Fixes franka_gripper_sim GripperCommand action

This commit makes sure that users can also send gripper widths between
0.0 and 0.08. In the old version the gripper would only open and close
based on whether the set gripper width was smaller or bigger than the
current width.
---
 franka_gazebo/config/franka_hw_sim.yaml  | 10 ++++-----
 franka_gazebo/src/franka_gripper_sim.cpp | 27 +++++++++++++++++-------
 2 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml
index 08a2f767a..1e573e8cb 100644
--- a/franka_gazebo/config/franka_hw_sim.yaml
+++ b/franka_gazebo/config/franka_hw_sim.yaml
@@ -1,12 +1,12 @@
 arm_id: $(arg arm_id)
 
-m_ee:  0.76
+m_ee: 0.76
 franka_gripper:
-  type:    franka_gazebo/FrankaGripperSim
-  arm_id:  $(arg arm_id)
+  type: franka_gazebo/FrankaGripperSim
+  arm_id: $(arg arm_id)
 
   finger1:
-    gains: { p: 100, i: 25, d: 20 }
+    gains: { p: 1000, i: 0, d: 0 }
 
   finger2:
-    gains: { p: 100, i: 25, d: 20 }
+    gains: { p: 1000, i: 0, d: 0 }
diff --git a/franka_gazebo/src/franka_gripper_sim.cpp b/franka_gazebo/src/franka_gripper_sim.cpp
index 117bd0327..96aca3de1 100644
--- a/franka_gazebo/src/franka_gripper_sim.cpp
+++ b/franka_gazebo/src/franka_gripper_sim.cpp
@@ -421,22 +421,35 @@ void FrankaGripperSim::onGraspGoal(const franka_gripper::GraspGoalConstPtr& goal
 }
 
 void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoalConstPtr& goal) {
+  control_msgs::GripperCommandResult result;
+  double width_d = goal->command.position * 2.0;
+
   ROS_INFO_STREAM_NAMED("FrankaGripperSim", "New Gripper Command Action Goal received: "
-                                                << goal->command.max_effort << "N");
+                                                << width_d << "m, " << goal->command.max_effort
+                                                << "N");
 
   // HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager
   // only sends us the width of one finger. Multiply by 2 to get the intended width.
   double width = this->finger1_.getPosition() + this->finger2_.getPosition();
 
+  if (width_d > kMaxFingerWidth || width_d < 0.0) {
+    std::string error =
+        "Commanding out of range width! max_width = " + std::to_string(kMaxFingerWidth) +
+        " command = " + std::to_string(width_d);
+    ROS_ERROR_STREAM_NAMED("FrankaGripperSim", error);
+    result.reached_goal = static_cast<decltype(result.reached_goal)>(false);
+    action_gc_->setAborted(result, error);
+    return;
+  }
+
   franka_gripper::GraspEpsilon eps;
   eps.inner = this->tolerance_gripper_action_;
   eps.outer = this->tolerance_gripper_action_;
 
-  transition(State::GRASPING,
-             Config{.width_desired = goal->command.position * 2.0 < width ? 0 : kMaxFingerWidth,
-                    .speed_desired = this->speed_default_,
-                    .force_desired = goal->command.max_effort,
-                    .tolerance = eps});
+  transition(State::GRASPING, Config{.width_desired = width_d,
+                                     .speed_desired = this->speed_default_,
+                                     .force_desired = goal->command.max_effort,
+                                     .tolerance = eps});
 
   waitUntilStateChange();
 
@@ -446,7 +459,6 @@ void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoa
     return;
   }
 
-  control_msgs::GripperCommandResult result;
   if (this->state_ != State::HOLDING) {
     result.reached_goal = static_cast<decltype(result.reached_goal)>(false);
     std::string error = "Unexpected state transistion: The gripper not in HOLDING as expected";
@@ -454,7 +466,6 @@ void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoa
     return;
   }
 
-  double width_d = goal->command.position * 2.0;
   width = this->finger1_.getPosition() + this->finger2_.getPosition();  // recalculate
   bool inside_tolerance = width_d - this->tolerance_gripper_action_ < width and
                           width < width_d + this->tolerance_gripper_action_;