Revert "Remove gripper epsilon"

This reverts commit 0635e31d.

CHANGELOG.md

@@ -5,6 +5,7 @@
 Requires `libfranka` >= 0.3.0
 
   * **BREAKING** Changed signatures in `franka_hw::FrankaModelHandle`
+  * **BREAKING** Added epsilon parameters to `franka_gripper/Grasp` action
   * Added missing dependency to `panda_moveit_config`
   * Fixed linker errors when building with `-DFranka_DIR` while an older version of
     `ros-kinetic-libfranka` is installed

franka_gripper/CMakeLists.txt

@@ -24,6 +24,11 @@ add_action_files(
         Move.action
 )
 
+add_message_files(
+  DIRECTORY msg
+  FILES GraspEpsilon.msg
+)
+
 generate_messages(DEPENDENCIES actionlib_msgs)
 
 catkin_package(

franka_gripper/action/Grasp.action

 float64 width    # [m]
+GraspEpsilon epsilon
 float64 speed    # [m/s]
 float64 force  # [N]
 ---

franka_gripper/config/gripper_node_config.yaml

@@ -3,3 +3,6 @@ joint_names:
   - panda_finger_joint1
   - panda_finger_joint2
 default_speed: 0.1  # [m/s]
+default_grasp_epsilon:
+  inner: 0.005 # [m]
+  outer: 0.005 # [m]
\ No newline at end of file

franka_gripper/include/franka_gripper/franka_gripper.h

@@ -35,11 +35,13 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
  *
  * @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.
  * @param[in] action_server A pointer to a gripper action server
  * @param[in] goal A gripper action goal
  */
 void gripperCommandExecuteCallback(
     const franka::Gripper& gripper,
+    const GraspEpsilon& grasp_epsilon,
     double default_speed,
     actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
     const control_msgs::GripperCommandGoalConstPtr& goal);
@@ -75,9 +77,11 @@ bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/);
 /**
  * Calls the libfranka grasp service of the gripper
  *
- * @param[in] gripper A gripper instance to execute the command
- * @param[in] goal A grasp goal with target width, velocity and effort
+ * An object is considered grasped if the distance \f$d\f$ between the gripper fingers satisfies
+ * \f$(\text{width} - \text{epsilon_inner}) < d < (\text{width} + \text{epsilon_outer})\f$.
  *
+ * @param[in] gripper A gripper instance to execute the command
+ * @param[in] goal A grasp goal with target width, epsilon_inner, epsilon_outer, velocity and effort
  * @return True if an object has been grasped, false otherwise.
  */
 bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal);

franka_gripper/msg/GraspEpsilon.msg

+float64 inner # [m]
+float64 outer # [m]

franka_gripper/src/franka_gripper.cpp

@@ -29,6 +29,7 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
 
 void gripperCommandExecuteCallback(
     const franka::Gripper& gripper,
+    const GraspEpsilon& grasp_epsilon,
     double default_speed,
     actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
     const control_msgs::GripperCommandGoalConstPtr& goal) {
@@ -42,7 +43,8 @@ void gripperCommandExecuteCallback(
     if (goal->command.position >= state.width) {
       return gripper.move(goal->command.position, default_speed);
     }
-    return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort);
+    return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort,
+                         grasp_epsilon.inner, grasp_epsilon.outer);
   };
 
   try {
@@ -77,7 +79,8 @@ bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/) {
 }
 
 bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal) {
-  return gripper.grasp(goal->width, goal->speed, goal->force);
+  return gripper.grasp(goal->width, goal->speed, goal->force, goal->epsilon.inner,
+                       goal->epsilon.outer);
 }
 
 }  // namespace franka_gripper

franka_gripper/src/franka_gripper_node.cpp

@@ -38,6 +38,7 @@ void handleErrors(actionlib::SimpleActionServer<T_action>* server,
 using actionlib::SimpleActionServer;
 using control_msgs::GripperCommandAction;
 using franka_gripper::GraspAction;
+using franka_gripper::GraspEpsilon;
 using franka_gripper::GraspGoalConstPtr;
 using franka_gripper::GraspResult;
 using franka_gripper::HomingAction;
@@ -70,6 +71,17 @@ int main(int argc, char** argv) {
     ROS_INFO_STREAM("franka_gripper_node: Found default_speed " << default_speed);
   }
 
+  GraspEpsilon default_grasp_epsilon;
+  default_grasp_epsilon.inner = 0.005;
+  default_grasp_epsilon.outer = 0.005;
+  std::map<std::string, double> epsilon_map;
+  if (node_handle.getParam("default_grasp_epsilon", epsilon_map)) {
+    ROS_INFO_STREAM("franka_gripper_node: Found default_grasp_epsilon "
+                    << "inner: " << epsilon_map["inner"] << ", outer: " << epsilon_map["outer"]);
+    default_grasp_epsilon.inner = epsilon_map["inner"];
+    default_grasp_epsilon.outer = epsilon_map["outer"];
+  }
+
   franka::Gripper gripper(robot_ip);
 
   std::function<bool(const HomingGoalConstPtr&)> homing_handler =
@@ -104,8 +116,8 @@ int main(int argc, char** argv) {
 
   SimpleActionServer<GripperCommandAction> gripper_command_action_server(
       node_handle, "gripper_action",
-      std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_speed,
-                &gripper_command_action_server, std::placeholders::_1),
+      std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_grasp_epsilon,
+                default_speed, &gripper_command_action_server, std::placeholders::_1),
       false);
 
   homing_action_server_.start();