strange pick behavior

asked 2014-05-06 19:16:45 -0500

jay75 gravatar image

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);
}
edit retag flag offensive close merge delete