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.
frankaemika · Oct 20, 2021 · a706b12 · a706b12
1 parent 38fa8a9
commit a706b12
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 11 deletions.
diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml
@@ -1,9 +1,9 @@
 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 }

diff --git 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,15 +459,13 @@ 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";
     action_gc_->setAborted(result, error);
     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_;