Remove gripper epsilon

CHANGELOG.md

@@ -3,7 +3,6 @@
 ## 0.2.0 - UNRELEASED
 
   * Added missing run-time dependencies to `franka_description` and `franka_control`
-  * Added epsilon to grasp action
   * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total`
     to the robot state
   * Updated and improved examples

franka_gripper/CMakeLists.txt

@@ -24,11 +24,6 @@ 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,6 +3,3 @@ 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,13 +35,11 @@ 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);
@@ -78,10 +76,9 @@ 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, epsilon_inner, epsilon_outer, velocity and effort
+ * @param[in] goal A grasp goal with target width, velocity and effort
  *
- * @return True if an object has been grasped, i.e.: the distance between the gripper fingers is
- * (width - epsilon_inner) < distance < (width + epsilon_outer), false otherwise.
+ * @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,7 +29,6 @@ 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) {
@@ -43,8 +42,7 @@ 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,
-                         grasp_epsilon.inner, grasp_epsilon.outer);
+    return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort);
   };
 
   try {
@@ -79,8 +77,7 @@ 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, goal->epsilon.inner,
-                       goal->epsilon.outer);
+  return gripper.grasp(goal->width, goal->speed, goal->force);
 }
 
 }  // namespace franka_gripper

franka_gripper/src/franka_gripper_node.cpp

@@ -38,7 +38,6 @@ 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;
@@ -71,17 +70,6 @@ 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 =
@@ -116,8 +104,8 @@ int main(int argc, char** argv) {
 
   SimpleActionServer<GripperCommandAction> gripper_command_action_server(
       node_handle, "gripper_action",
-      std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_grasp_epsilon,
-                default_speed, &gripper_command_action_server, std::placeholders::_1),
+      std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_speed,
+                &gripper_command_action_server, std::placeholders::_1),
       false);
 
   homing_action_server_.start();