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

@@ -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 }

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_;