From 344534c121758a667a0aba291f412e42421d906f Mon Sep 17 00:00:00 2001
From: Florian Walch <florian.walch@franka.de>
Date: Mon, 19 Mar 2018 11:27:03 +0100
Subject: [PATCH] Add workaround for commanding correct gripper width from
MoveIt
---
CHANGELOG.md | 1 +
.../include/franka_gripper/franka_gripper.h | 4 ++++
franka_gripper/src/franka_gripper.cpp | 18 +++++++++++-------
.../config/panda_gripper_controllers.yaml | 1 -
4 files changed, 16 insertions(+), 8 deletions(-)
diff --git a/CHANGELOG.md b/CHANGELOG.md
index d62af09..a576401 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 6828577..305d069 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 86a01d1..7403426 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 f31e6be..1b23184 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
--
GitLab