Close Franka gripper to a width C++

edit

Hi, I I'm using Franka Emika 0.8.0 version with Rviz simulator. I'm running Ubuntu 20.4 on a Lenovo Thinkpad with kernel 5.4.123-rt59.

So I am running a pick and place execution with the robot. My code is following:

std::vector<moveit_msgs::Grasp> grasp;
grasp.resize(1);
grasp[0].grasp_pose.header.frame_id = "panda_link0";

// Setting grasp pose
grasp[0].grasp_pose.pose = from;
std::cout<<"from pose: " <<from;
grasp[0].grasp_pose.pose.position.z += 0.1;

Eigen::Quaternionf q1(grasp[0].grasp_pose.pose.orientation.w,grasp[0].grasp_pose.pose.orientation.x,grasp[0].grasp_pose.pose.orientation.y,grasp[0].grasp_pose.pose.orientation.z);
Eigen::Quaternionf q2(0.9238795, 0, 0, -0.3826834);
Eigen::Quaternionf result_quat = q2*q1;
grasp[0].grasp_pose.pose.orientation.w = q1.w();
grasp[0].grasp_pose.pose.orientation.x = q1.x();
grasp[0].grasp_pose.pose.orientation.y = q1.y();
grasp[0].grasp_pose.pose.orientation.z = q1.z();


// /* Direction is set as positive z axis */
grasp[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
grasp[0].pre_grasp_approach.direction.vector.z = -1.0;
grasp[0].pre_grasp_approach.min_distance = 0.02;
grasp[0].pre_grasp_approach.desired_distance = 0.115;
/* Defined with respect to frame_id */
grasp[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as positive z axis */
grasp[0].post_grasp_retreat.direction.vector.z = 1.0;
grasp[0].post_grasp_retreat.min_distance = 0.02;
grasp[0].post_grasp_retreat.desired_distance = 0.15;

/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";

/* Set them as open, wide enough for the object to fit. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = plugW/2; //+ 0.005;
posture.points[0].positions[1] = plugW/2; //+ 0.0045;
posture.points[0].time_from_start = ros::Duration(0.5);

/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";

/* Set them as closed. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.0;
posture.points[0].positions[1] = 0.0;
posture.points[0].time_from_start = ros::Duration(0.5);

closedGripper(grasp[0].grasp_posture);

I want to close the gripper to a specific width and not close up fully. Our gripper is going to pick up a very soft object, so there is no resistance for the gripper to stop closing. Is there any solution for this?

I tried this:

 posture.points[0].positions[0] = plugW/2 - 0.005;
 posture.points[0].positions[1] = plugW/2 - 0.005;

but the movement did not go through.