Add workaround for commanding correct gripper width from MoveIt

CHANGELOG.md

@@ -4,6 +4,7 @@
 
   * **BREAKING** Removed `arm_id` from launchfiles
   * **BREAKING** Changed namespace of `franka_control` controller manager
+  * **BREAKING** Changed behavior of `gripper_action` for compatibility with MoveIt
   * Changes in `panda_moveit_config`:
     * Updated joint limits from URDF
     * Removed `home` poses

franka_gripper/include/franka_gripper/franka_gripper.h

@@ -33,6 +33,10 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
  * Wraps the execution of a gripper command action to catch exceptions and
  * report results
  *
+ * @note
+ * For compatibility with current MoveIt! behavior, the given goal's command position is
+ * multiplied by a factor of 2 before being commanded to the gripper!
+ *
  * @param[in] gripper A pointer to a franka gripper
  * @param[in] default_speed The default speed for a gripper action
  * @param[in] grasp_epsilon The epsilon window of the grasp.

franka_gripper/src/franka_gripper.cpp

@@ -35,20 +35,24 @@ void gripperCommandExecuteCallback(
     const control_msgs::GripperCommandGoalConstPtr& goal) {
   constexpr double kSamePositionThreshold = 1e-4;
 
-  std::function<bool()> gripper_command_handler = [=, &gripper]() {
+  auto gripper_command_handler = [goal, grasp_epsilon, default_speed, &gripper]() {
+    // 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 target_width = 2 * goal->command.position;
+
     franka::GripperState state = gripper.readOnce();
-    if (goal->command.position > state.max_width || goal->command.position < 0.0) {
+    if (target_width > state.max_width || target_width < 0.0) {
       ROS_ERROR_STREAM("GripperServer: Commanding out of range width! max_width = "
-                       << state.max_width << " command = " << goal->command.position);
+                       << state.max_width << " command = " << target_width);
       return false;
     }
-    if (std::abs(goal->command.position - state.width) < kSamePositionThreshold) {
+    if (std::abs(target_width - state.width) < kSamePositionThreshold) {
       return true;
     }
-    if (goal->command.position >= state.width) {
-      return gripper.move(goal->command.position, default_speed);
+    if (target_width >= state.width) {
+      return gripper.move(target_width, default_speed);
     }
-    return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort,
+    return gripper.grasp(target_width, default_speed, goal->command.max_effort,
                          grasp_epsilon.inner, grasp_epsilon.outer);
   };
 

panda_moveit_config/config/panda_gripper_controllers.yaml

@@ -13,7 +13,6 @@ controller_list:
   - name: franka_gripper
     action_ns: gripper_action
     type: GripperCommand
-    parallel: true
     joints:
       - panda_finger_joint1
       - panda_finger_joint2