integrated force adaption by parameter

src/franka_gripper.cpp

@@ -33,6 +33,7 @@ void gripperCommandExecuteCallback(
     double default_speed,
     actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
     const control_msgs::GripperCommandGoalConstPtr& goal) {
+
   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.
@@ -51,7 +52,19 @@ void gripperCommandExecuteCallback(
     if (target_width >= state.width) {
       return gripper.move(target_width, default_speed);
     }
-    return gripper.grasp(target_width, default_speed, goal->command.max_effort, grasp_epsilon.inner,
+
+    double force = 0.0;
+
+      if(!ros::param::has("tud_grasp_force")){
+          ros::param::set("tud_grasp_force", 0.0);
+          force = 0.0;
+      }else{
+          ros::param::get("tud_grasp_force", force);
+      }
+
+      std::cout << "force: " << force << std::endl;
+
+    return gripper.grasp(target_width, default_speed, force /*goal->command.max_effort*/, grasp_epsilon.inner,
                          grasp_epsilon.outer);
   };