strange pick behavior
please check out the attached video : clamrviz.mov
why does the arm pop back straight up, and then do the pick?
using hydro, ubuntu 12.04
here is the code:
bool pick(const geometry_msgs::Pose& block_pose, std::string block_name)
{
ROS_WARN_STREAM_NAMED("","picking block "<< block_name);
std::vector<moveit_msgs::Grasp> grasp;
grasp.resize(1);
grasp[0].id = "rightarm_grasp";
grasp[0].pre_grasp_posture.header.frame_id = BASE_LINK;
grasp[0].pre_grasp_posture.header.stamp = ros::Time::now();
grasp[0].pre_grasp_posture.joint_names.resize(1);
grasp[0].pre_grasp_posture.joint_names[0] = EE_JOINT;
grasp[0].pre_grasp_posture.points.resize(1);
grasp[0].pre_grasp_posture.points[0].positions.resize(1);
grasp[0].pre_grasp_posture.points[0].positions[0] = clam_msgs::ClamGripperCommandGoal::GRIPPER_OPEN;
grasp[0].grasp_posture.header.frame_id = BASE_LINK;
grasp[0].grasp_posture.header.stamp = ros::Time::now();
grasp[0].grasp_posture.joint_names.resize(1);
grasp[0].grasp_posture.joint_names[0] = EE_JOINT;
grasp[0].grasp_posture.points.resize(1);
grasp[0].grasp_posture.points[0].positions.resize(1);
grasp[0].grasp_posture.points[0].positions[0] = clam_msgs::ClamGripperCommandGoal::GRIPPER_CLOSE;
grasp[0].grasp_pose.header.stamp = ros::Time::now();
grasp[0].grasp_pose.header.frame_id = BASE_LINK;
grasp[0].grasp_pose.pose.position.x = block_pose.position.x-0.15;
grasp[0].grasp_pose.pose.position.y = block_pose.position.y;
grasp[0].grasp_pose.pose.position.z = 0.2;
grasp[0].grasp_pose.pose.orientation.x = 0.0;
grasp[0].grasp_pose.pose.orientation.y = 0.0;
grasp[0].grasp_pose.pose.orientation.z = 0.0;
grasp[0].grasp_pose.pose.orientation.w = 1.0;
grasp[0].pre_grasp_approach.direction.header.frame_id = BASE_LINK;
grasp[0].pre_grasp_approach.direction.header.stamp = ros::Time::now();
grasp[0].pre_grasp_approach.direction.vector.x = 1;
grasp[0].pre_grasp_approach.direction.vector.y = 0;
grasp[0].pre_grasp_approach.direction.vector.z = -1;
grasp[0].pre_grasp_approach.desired_distance = 0.05;
grasp[0].pre_grasp_approach.min_distance = 0.025;
grasp[0].post_grasp_retreat.direction.header.frame_id = BASE_LINK;
grasp[0].post_grasp_retreat.direction.header.stamp = ros::Time::now();
grasp[0].post_grasp_retreat.direction.vector.x = -1;
grasp[0].post_grasp_retreat.direction.vector.y = 0;
grasp[0].post_grasp_retreat.direction.vector.z = 1;
grasp[0].post_grasp_retreat.desired_distance = 0.05;
grasp[0].post_grasp_retreat.min_distance = 0.025;
grasp[0].post_place_retreat.direction.header.frame_id = BASE_LINK;
grasp[0].post_place_retreat.direction.header.stamp = ros::Time::now();
grasp[0].post_place_retreat.direction.vector.x = -1;
grasp[0].post_place_retreat.direction.vector.y = 0;
grasp[0].post_place_retreat.direction.vector.z = 1;
grasp[0].post_place_retreat.desired_distance = 0.05;
grasp[0].post_place_retreat.min_distance = 0.025;
grasp[0].allowed_touch_objects.resize(1);
grasp[0].allowed_touch_objects[0] = block_name;
return group_->pick(block_name, grasp);
}