diff --git a/CHANGELOG.md b/CHANGELOG.md index d62af0998eab060a0158f3bc1e6e6554eccfcc0e..a576401fdf7933f5ad0e2576605ba6ef93827d6b 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/franka_gripper/include/franka_gripper/franka_gripper.h b/franka_gripper/include/franka_gripper/franka_gripper.h index 6828577e497d2b5a7e7ab668cd8ef559c9839592..305d0698993bded6c2a78604f26ef93adc62052d 100644 --- a/franka_gripper/include/franka_gripper/franka_gripper.h +++ b/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. diff --git a/franka_gripper/src/franka_gripper.cpp b/franka_gripper/src/franka_gripper.cpp index 86a01d172aee40cf94bb27f99d1fc7d118d83de5..74034268767711b7b6f1e7f6fcddd9088c7f4559 100644 --- a/franka_gripper/src/franka_gripper.cpp +++ b/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); }; diff --git a/panda_moveit_config/config/panda_gripper_controllers.yaml b/panda_moveit_config/config/panda_gripper_controllers.yaml index f31e6beb6b8d1953ace5a6869d55263aa4486234..1b2318485469afbf32bddb82438484d842d8bc94 100644 --- a/panda_moveit_config/config/panda_gripper_controllers.yaml +++ b/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