Example #1
0
void demoPick(move_group_interface::MoveGroup &group)
{
  std::vector<manipulation_msgs::Grasp> grasps;
  for (std::size_t i = 0 ; i < 20 ; ++i)
  {
    geometry_msgs::PoseStamped p = group.getRandomPose();
    p.pose.orientation.x = 0;
    p.pose.orientation.y = 0;
    p.pose.orientation.z = 0;
    p.pose.orientation.w = 1;
    manipulation_msgs::Grasp g;
    g.grasp_pose = p;
    g.approach.direction.vector.x = 1.0;
    g.retreat.direction.vector.z = 1.0;
    g.retreat.direction.header = p.header;
    g.approach.min_distance = 0.2;
    g.approach.desired_distance = 0.4;
    g.retreat.min_distance = 0.1;
    g.retreat.desired_distance = 0.27;
    g.pre_grasp_posture.name.resize(1, "r_gripper_joint");
    g.pre_grasp_posture.position.resize(1);
    g.pre_grasp_posture.position[0] = 1;

    g.grasp_posture.name.resize(1, "r_gripper_joint");
    g.grasp_posture.position.resize(1);
    g.grasp_posture.position[0] = 0;

    grasps.push_back(g);
  }
  group.pick("bubu", grasps);
}
void pick(move_group_interface::MoveGroup &group)
{
  std::vector<manipulation_msgs::Grasp> grasps;
  
  geometry_msgs::PoseStamped p;
  p.header.frame_id = "base_footprint";
  p.pose.position.x = 0.32;
  p.pose.position.y = -0.7;
  p.pose.position.z = 0.5;
  p.pose.orientation.x = 0;
  p.pose.orientation.y = 0;
  p.pose.orientation.z = 0;
  p.pose.orientation.w = 1;
  manipulation_msgs::Grasp g;
  g.grasp_pose = p;
  
  g.approach.direction.vector.x = 1.0;
  g.approach.direction.header.frame_id = "r_wrist_roll_link";
  g.approach.min_distance = 0.2;
  g.approach.desired_distance = 0.4;

  g.retreat.direction.header.frame_id = "base_footprint";
  g.retreat.direction.vector.z = 1.0;
  g.retreat.min_distance = 0.1;
  g.retreat.desired_distance = 0.25;

  g.pre_grasp_posture.name.resize(1, "r_gripper_joint");
  g.pre_grasp_posture.position.resize(1);
  g.pre_grasp_posture.position[0] = 1;
  
  g.grasp_posture.name.resize(1, "r_gripper_joint");
  g.grasp_posture.position.resize(1);
  g.grasp_posture.position[0] = 0;
  
  grasps.push_back(g);
  group.setSupportSurfaceName("table");
  group.pick("part", grasps);
}
Example #3
0
	int pickup() {
		/*
		 // ----- pick up object near point (in meter) relative to base_footprint
		 geometry_msgs::PointStamped pickup_point;
		 pickup_point.header.frame_id = KURTANA_BASE_LINK;
		 pickup_point.point.x = desired_pickup_point.x;
		 pickup_point.point.y = desired_pickup_point.y;
		 pickup_point.point.z = desired_pickup_point.z;
		 */

		// ----- call the tabletop detection
		ROS_INFO("Calling tabletop detector");
		tabletop_object_detector::TabletopDetection detection_call;
		//we want recognized database objects returned
		//set this to false if you are using the pipeline without the database
		detection_call.request.return_models = false;

		//we want the individual object point clouds returned as well
		detection_call.request.return_clusters = false;

		if (!object_detection_srv.call(detection_call)) {
			ROS_ERROR("Tabletop detection service failed");
			return -1;
		}
		if (detection_call.response.detection.result
				!= detection_call.response.detection.SUCCESS) {
			ROS_ERROR(
					"Tabletop detection returned error code %d", detection_call.response.detection.result);
			return -1;
		}
		if (detection_call.response.detection.clusters.empty()
				&& detection_call.response.detection.models.empty()) {
			ROS_ERROR(
					"The tabletop detector detected the table, but found no objects");
			return -1;
		}

		//Remove the table because there are convex hull problems if adding the table to envirnonment
		//detection_call.response.detection.table.convex_hull = shape_msgs::Mesh();

		tabletop_collision_map_processing::TabletopCollisionMapProcessing process_call;

		process_call.request.detection_result = detection_call.response.detection;
		process_call.request.reset_collision_models = false;
		process_call.request.reset_attached_models = false;

		if (!collision_processing_srv.call(process_call)) {
				ROS_ERROR("Tabletop Collision Map Processing failed");
				return -1;
		}

		ROS_INFO_STREAM("Found objects count: " << process_call.response.collision_object_names.size());

		if (process_call.response.collision_object_names.empty()) {
				ROS_ERROR("Tabletop Collision Map Processing error");
				return -1;
		}

		/*

		ROS_INFO("Add table to planning scene");

		moveit_msgs::CollisionObject collision_object;

		collision_object.header.stamp = ros::Time::now();
		collision_object.header.frame_id =
				detection_call.response.detection.table.pose.header.frame_id;



		ROS_INFO_STREAM("Add clusters");

		object_positions.clear();

		//add objects to planning scene
		for (unsigned int i = 0;
				i < detection_call.response.detection.clusters.size(); i++) {

			sensor_msgs::PointCloud2 pc2;
			sensor_msgs::convertPointCloudToPointCloud2(
					detection_call.response.detection.clusters[i], pc2);
			geometry_msgs::PoseStamped poseStamped;
			geometry_msgs::Vector3 dimension;
			getClusterBoundingBox3D(pc2, poseStamped, dimension);

			geometry_msgs::PointStamped point;

			point.header.frame_id = poseStamped.header.frame_id;
			point.point = poseStamped.pose.position;

			object_positions.push_back(point);

			ostringstream id;
			id << "object " << i;
			collision_object.id = id.str().c_str();

			ROS_INFO_STREAM("Object id: " << collision_object.id);

			collision_object.operation = moveit_msgs::CollisionObject::REMOVE;

			pub_collision_object.publish(collision_object);

			collision_object.operation = moveit_msgs::CollisionObject::ADD;
			collision_object.primitives.resize(1);
			collision_object.primitives[0].type =
					shape_msgs::SolidPrimitive::BOX;
			collision_object.primitives[0].dimensions.resize(
					shape_tools::SolidPrimitiveDimCount<
							shape_msgs::SolidPrimitive::BOX>::value);
			collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] =
					dimension.x;
			collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] =
					dimension.y;
			collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] =
					dimension.z;
			collision_object.primitive_poses.resize(1);
			collision_object.primitive_poses[0] = poseStamped.pose;

			pub_collision_object.publish(collision_object);

		}

		*/

		//group->setSupportSurfaceName("table");

		//call object pickup
		ROS_INFO("Calling the pickup action");

		generated_pickup_grasp = generateGrasp();

		ROS_INFO("Get nearest object");

		object_to_manipulate = nearest_object();


		//test

		/*
[DEBUG] [1378456372.242171284]: IK pose: position:
  x: 0.44866
  y: -0.185202
  z: 0.392208
orientation:
  x: 0.00125435
  y: -0.00632681
  z: -0.194471
  w: 0.980887

[DEBUG] [1378456372.242351955]: Found 4 solutions from IKFast
[DEBUG] [1378456372.242443678]: Sol 0: -3.914825e-01   1.548975e+00   -1.572923e+00   -3.587233e-02   -4.921995e-03   2.618548e-322
[DEBUG] [1378456372.243036938]: Solution passes callback


		geometry_msgs::PoseStamped p;
		p.header.frame_id = ARM_BASE_LINK;
		p.pose.position.x = 0.44866;
		p.pose.position.y = -0.185202;
		p.pose.position.z = 0.392208;


		p.pose.orientation.x = 0;
		p.pose.orientation.y = 0;
		p.pose.orientation.z = 0;
		p.pose.orientation.w = 1;

		group->setPoseTarget(p,GRIPPER_FRAME);

		group->move();
*/
		//group->pick(object_to_manipulate);

		ROS_INFO_STREAM("Picking up Object: " << object_to_manipulate);

		group->pick(object_to_manipulate, generated_pickup_grasp);

		ROS_INFO("Pick returned!!!!!11111 OMGWTFIT");

		//group->place(object_to_manipulate);

		std::vector<double> rpy = group->getCurrentRPY(
				group->getEndEffectorLink());

		ROS_INFO_STREAM(
				"End effector link: " << rpy.at(0)<< rpy.at(1) << rpy.at(2));




		object_in_gripper = 1;

		move_arm_out_of_the_way();



		return 0;

	}