gripper_action: Don't move if command position has already been reached

franka_gripper/src/franka_gripper.cpp

@@ -33,6 +33,8 @@ void gripperCommandExecuteCallback(
     double default_speed,
     actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
     const control_msgs::GripperCommandGoalConstPtr& goal) {
+  constexpr double kSamePositionThreshold = 1e-4;
+
   std::function<bool()> gripper_command_handler = [=, &gripper]() {
     franka::GripperState state = gripper.readOnce();
     if (goal->command.position > state.max_width || goal->command.position < 0.0) {
@@ -40,6 +42,9 @@ void gripperCommandExecuteCallback(
                        << state.max_width << " command = " << goal->command.position);
       return false;
     }
+    if (std::abs(goal->command.position - state.width) < kSamePositionThreshold) {
+      return true;
+    }
     if (goal->command.position >= state.width) {
       return gripper.move(goal->command.position, default_speed);
     }