PR2 stuck picking object

edit

Hey everybody.

I'm working on a grasping node, using a PR2 robot simulated in Gazebo (Ubuntu 12.04 + Hydro). So far I have been able to generate a pickup goal and a grasp message that allows me to "attempt" to grasp an object (I have tested the approach directions, grasping pose, retreat pose, etc), but every time I try to pick a simulated object the robot gets stuck closing the gripper when trying to grasp the object. I've tried to grasp the object using MoveIt and the MoveGroup interface and also using an ActionClient, but in both cases the result is the same.

After some debug I noticed that the problem is that the gripper is closing but the object is blocking it (of course), so it never reaches the closing goal and that makes the grasping attempt to timeout.

Despite a lot of reading and experimentation I haven't been able to the generate a grasp that doesn't ask the gripper to be fully closed. The grasp message specifies a grasping posture where the joints positions can be given, but despite any value I'm only able to generate postures indicating the gripper fully opened or fully close.

The current message I'm sending is something like the following:

moveit_msgs::Grasp grasp;
grasp.id = "TARGET_GRASP";
grasp.grasp_pose = graspPose;

grasp.pre_grasp_approach.direction.header.frame_id = "r_wrist_roll_link";
grasp.pre_grasp_approach.direction.vector.x = 1;
grasp.pre_grasp_approach.direction.vector.y = 0;
grasp.pre_grasp_approach.direction.vector.z = 0;
grasp.pre_grasp_approach.min_distance = 0.05;
grasp.pre_grasp_approach.desired_distance = 0.18;

grasp.pre_grasp_posture.joint_names.resize(1, "r_gripper_motor_screw_joint");
grasp.pre_grasp_posture.points.resize(1);
grasp.pre_grasp_posture.points[0].positions.resize(1);
grasp.pre_grasp_posture.points[0].positions[0] = 0.8;
grasp.pre_grasp_posture.points[0].time_from_start = ros::Duration(45.0);

grasp.grasp_posture.joint_names.resize(1, "r_gripper_motor_screw_joint");
grasp.grasp_posture.points.resize(1);
grasp.grasp_posture.points[0].positions.resize(1);
grasp.grasp_posture.points[0].positions[0] = 0.01;
grasp.grasp_posture.points[0].time_from_start = ros::Duration(45.0);

grasp.post_grasp_retreat.direction.header.frame_id = "base_footprint";
grasp.post_grasp_retreat.direction.vector.x = 0;
grasp.post_grasp_retreat.direction.vector.y = 0;
grasp.post_grasp_retreat.direction.vector.z = 1;
grasp.post_grasp_retreat.min_distance = 0.08;
grasp.post_grasp_retreat.desired_distance = 0.3;

grasp.allowed_touch_objects.clear();
grasp.allowed_touch_objects.push_back(TARGET_ID);
grasp.allowed_touch_objects.push_back(SUPPORT_ID);

So, do anybody have an idea whats the problem here?? In my launch file I have given the following parameters:

<param name="stall_velocity_threshold" type="double" value="0.001"/>
<param name="stall_timeout" type="double" value="0.5"/>
<param name="goal_threshold" type="double" value="0.01"/>

I understand that those are used for the gripper stall detection. I changed them because the default values (stall_velocity_threshold = 0.2) were causing the gripper client to detect a false stall, aborting the grasp action prematurely and messing with the grasping attempt.

Any help is greatly appreciated!!