示例#1
0
void demoPlace(move_group_interface::MoveGroup &group)
{
  std::vector<manipulation_msgs::PlaceLocation> loc;
  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::PlaceLocation g;
    g.place_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.post_place_posture.name.resize(1, "r_gripper_joint");
    g.post_place_posture.position.resize(1);
    g.post_place_posture.position[0] = 0;
    
    loc.push_back(g);
  }
  group.place("bubu", loc);
}
void place(move_group_interface::MoveGroup &group)
{
  std::vector<manipulation_msgs::PlaceLocation> loc;
  
  geometry_msgs::PoseStamped p; 
  p.header.frame_id = "base_footprint";
  p.pose.position.x = 0.42;
  p.pose.position.y = 0.0;
  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::PlaceLocation g;
  g.place_pose = p;  
  
  g.approach.direction.vector.z = -1.0;
  g.retreat.direction.vector.x = -1.0;
  g.retreat.direction.header.frame_id = "base_footprint";
  g.approach.direction.header.frame_id = "r_wrist_roll_link";
  g.approach.min_distance = 0.1;
  g.approach.desired_distance = 0.2;
  g.retreat.min_distance = 0.1;
  g.retreat.desired_distance = 0.25;
  
  g.post_place_posture.name.resize(1, "r_gripper_joint");
  g.post_place_posture.position.resize(1);
  g.post_place_posture.position[0] = 1;
  
  loc.push_back(g);
  group.setSupportSurfaceName("table");


  // add path constraints
  moveit_msgs::Constraints constr;
  constr.orientation_constraints.resize(1);
  moveit_msgs::OrientationConstraint &ocm = constr.orientation_constraints[0];
  ocm.link_name = "r_wrist_roll_link";
  ocm.header.frame_id = p.header.frame_id;
  ocm.orientation.x = 0.0;
  ocm.orientation.y = 0.0;
  ocm.orientation.z = 0.0;
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.2;
  ocm.absolute_y_axis_tolerance = 0.2;
  ocm.absolute_z_axis_tolerance = M_PI;
  ocm.weight = 1.0; 
  group.setPathConstraints(constr);  
  group.setPlannerId("RRTConnectkConfigDefault");
  
  group.place("part", loc);
}
示例#3
0
	void set_joint_goal() {

		group->setJointValueTarget("katana_motor1_pan_joint", -1.51);
		group->setJointValueTarget("katana_motor2_lift_joint",
				2.13549384276445);
		group->setJointValueTarget("katana_motor3_lift_joint",
				-2.1556486321117725);
		group->setJointValueTarget("katana_motor4_lift_joint",
				-1.971949347057968);
		group->setJointValueTarget("katana_motor5_wrist_roll_joint", 0.0);

		return;
	}
示例#4
0
void demoFollowConstraints(move_group_interface::MoveGroup &group)
{
  geometry_msgs::PoseStamped curr = group.getCurrentPose();
  
  std::vector<geometry_msgs::Pose> c(2);
  c[0] = curr.pose;
  c[1] = c[0];
  c[1].position.x -= 0.01;
  
  group.followConstraints(c);
  move_group_interface::MoveGroup::Plan p;  
  group.plan(p); 
}
void move(ros::Publisher pub, move_group_interface::MoveGroup group, std::vector<double> joints_target, std::vector<std::string> joint_names, double speed, double thres_in)
{
	std::vector<double> joints_curr;
	std::vector<double> joint_error;
	std::vector<double> joint_vel_com;

	double error_magnitude;
	double alpha = 1; // step size
	double joint_vel_com_mag;

	double max_vel_mag = 0.08;
	if (speed < max_vel_mag)
		max_vel_mag = speed;

	double thres = 0.01;
	if (thres_in < thres)
		thres = thres_in;

	for (int i=0; i< 100000; i++)
	{
		//~ std::cout << "hi 1" << std::endl;
		joints_curr = group.getCurrentJointValues();
		joint_error = joints_curr;
		joint_vel_com = joints_curr;

		//~ std::cout << "hi 2" << std::endl;
		// calculate joint error, error magnitude, comm vel
		error_magnitude = 0;
		for (int j=0; j< joint_error.size(); j++) {
			joint_error[j] = joints_target[j] - joints_curr[j];
			joint_vel_com[j] = joint_error[j] * alpha;
			error_magnitude += joint_error[j]*joint_error[j];
		}
		//~ std::cout << "hi 3" << std::endl;
		error_magnitude = sqrt(error_magnitude);
		joint_vel_com_mag = error_magnitude*alpha;

		//~ std::cout << "hi 4" << std::endl;
		// if comm vel exceeds max, scale it back to max
		if (joint_vel_com_mag > max_vel_mag)
			for (int j=0; j< joint_vel_com.size(); j++)
				joint_vel_com[j] = max_vel_mag * joint_vel_com[j] / joint_vel_com_mag;

		//~ std::cout << "hi 5" << std::endl;
		setJointValues(pub, baxter_core_msgs::JointCommand::VELOCITY_MODE, joint_vel_com, joint_names);
		std::cout << "iter: " << i << " error_magnitude: " << error_magnitude << std::endl;
		//~ std::cout << "hi 6" << std::endl;

		if (error_magnitude < thres)
			break;

		//~ std::cout << "hi 7" << std::endl;
		usleep(1000);
	}

	// need to reset to position mode when we're done or the robot will disable itself...
	setJointValues(pub, baxter_core_msgs::JointCommand::POSITION_MODE, joints_target, joint_names);

}
示例#6
0
	int move_arm_out_of_the_way() {

		//set_joint_goal();

		//clear_collision_map();

		ROS_INFO("Move arm out of the way.");

		//move arm to initial home state as defined in the urdf
		group->setNamedTarget("home_stable");

		group->asyncMove();

		//clear_collision_map();

		ROS_INFO("Arm moved out of the way.");

		return 1;
	}
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);
}
void move_to_wait_position(move_group_interface::MoveGroup& move_group)
{
  //ROS_ERROR_STREAM("move_to_wait_position is not implemented yet.  Aborting."); exit(1);

  // task variables
  bool success; // saves the move result

  // set robot wait target
  /* Fill Code: [ use the 'setNamedTarget' method in the 'move_group' object] */
  move_group.setNamedTarget(cfg.WAIT_POSE_NAME);

  // move the robot
  /* Fill Code: [ use the 'move' method in the 'move_group' object and save the result in the 'success' variable] */
  success = move_group.move();
  if(success)
  {
    ROS_INFO_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Succeeded");
  }
  else
  {
    ROS_ERROR_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Failed");
    exit(1);
  }
}
示例#9
0
	bool place() {



		//group->place(object_to_manipulate, generated_pickup_grasp.grasp_pose);


		std::vector<manipulation_msgs::PlaceLocation> loc;

		geometry_msgs::PoseStamped p;
		/*
		p.header.frame_id = BASE_LINK;
		p.pose.position = object_to_manipulate_position.point;

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

		*/
		p = generated_pickup_grasp.grasp_pose;


		make_pose_reachable_by_5DOF_katana(p);

		ROS_DEBUG_STREAM("Place Pose: " << p.pose);


		manipulation_msgs::PlaceLocation g;
		g.place_pose = p;

		g.approach.direction.vector.z = -1.0;
		g.retreat.direction.vector.x = -1.0;
		g.retreat.direction.header.frame_id = BASE_LINK;
		g.approach.direction.header.frame_id = GRIPPER_FRAME;
		g.approach.min_distance = 0.1;
		g.approach.desired_distance = 0.2;
		g.retreat.min_distance = 0.1;
		g.retreat.desired_distance = 0.25;

		g.post_place_posture.name.resize(1, FINGER_JOINT);
		g.post_place_posture.position.resize(1);
		g.post_place_posture.position[0] = 0.30;


		loc.push_back(g);

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

		/* Option path constraints (e.g. to always stay upright)
		// add path constraints
		moveit_msgs::Constraints constr;
		constr.orientation_constraints.resize(1);
		moveit_msgs::OrientationConstraint &ocm = constr.orientation_constraints[0];
		ocm.link_name = "r_wrist_roll_link";
		ocm.header.frame_id = p.header.frame_id;
		ocm.orientation.x = 0.0;
		ocm.orientation.y = 0.0;
		ocm.orientation.z = 0.0;
		ocm.orientation.w = 1.0;
		ocm.absolute_x_axis_tolerance = 0.2;
		ocm.absolute_y_axis_tolerance = 0.2;
		ocm.absolute_z_axis_tolerance = M_PI;
		ocm.weight = 1.0;
		group->setPathConstraints(constr);
		group->setPlannerId("RRTConnectkConfigDefault");
		*/

		group->place(object_to_manipulate, loc);



		/*
		 //create a place location
		 //geometry_msgs::PoseStamped place_location = pickup_location;

		 geometry_msgs::PoseStamped place_location;

		 place_location.header.stamp = ros::Time::now();

		 // ----- put the object down
		 ROS_INFO("Calling the place action");
		 object_manipulation_msgs::PlaceGoal place_goal;

		 place_location.header.frame_id = KATANA_BASE_LINK;



		 place_location.pose.position.x =
		 normalPoseRobotFrame.pose.position.x;
		 place_location.pose.position.y =
		 normalPoseRobotFrame.pose.position.y;
		 place_location.pose.position.z =
		 pickup_result.grasp.grasp_pose.position.z; //TODO: to calculate z object higth needs to be considered

		 tf::Transform position;
		 tf::poseMsgToTF(place_location.pose, position);

		 tf::Vector3 shift_direction;
		 shift_direction.setX(place_location.pose.position.x);
		 shift_direction.setY(place_location.pose.position.y);
		 shift_direction.setZ(0);
		 shift_direction.normalize();

		 tf::Vector3 shift_direction_inverse;
		 shift_direction_inverse.setX(-shift_direction.getX());
		 shift_direction_inverse.setY(-shift_direction.getY());
		 shift_direction_inverse.setZ(-shift_direction.getZ());

		 //transform place_position to the start of the test line
		 tf::Transform trans(tf::Quaternion(0, 0, 0, 1.0),
		 shift_direction_inverse * place_position_tolerance_in_meter);

		 position = trans * position;

		 //move along the test line and add positions to place_goal.place_locations
		 for (float offset = 0; offset <= place_position_tolerance_in_meter;
		 offset += place_planner_step_size_in_meter) {

		 tf::Transform trans(tf::Quaternion(0, 0, 0, 1.0),
		 shift_direction * place_planner_step_size_in_meter);

		 position = trans * position;

		 geometry_msgs::Pose position_pose;
		 tf::poseTFToMsg(position, position_pose);

		 place_location.pose = position_pose;

		 // Convert quaternion to RPY.
		 tf::Quaternion q;
		 double roll, pitch, yaw;
		 tf::quaternionMsgToTF(pickup_result.grasp.grasp_pose.orientation,
		 q);
		 tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

		 //determine yaw which is compatible with the Katana 300 180 kinematics.
		 yaw = atan2(place_location.pose.position.y,
		 place_location.pose.position.x);

		 tf::Quaternion quat = tf::createQuaternionFromRPY(0.0, pitch, yaw);

		 place_location.pose.orientation.x = quat.getX();
		 place_location.pose.orientation.y = quat.getY();
		 place_location.pose.orientation.z = quat.getZ();
		 place_location.pose.orientation.w = quat.getW();

		 ROS_INFO(
		 "Place Location: XYZ, Quaternions xyzw = %lf,%lf,%lf, (%lf, %lf, %lf, %lf)", place_location.pose.position.x, place_location.pose.position.y, place_location.pose.position.z, place_location.pose.orientation.x, place_location.pose.orientation.y, place_location.pose.orientation.z, place_location.pose.orientation.w);

		 place_goal.place_locations.push_back(place_location);

		 }

		 //end place planner

		 //the collision names of both the objects and the table
		 //same as in the pickup action
		 place_goal.collision_object_name =
		 processing_call.response.collision_object_names.at(
		 object_to_pick_ind);
		 place_goal.collision_support_surface_name =
		 processing_call.response.collision_support_surface_name;

		 //set grasp orientation to identity (all info is already given by place_location)

		 place_goal.grasp.grasp_pose.orientation.x = 0;
		 place_goal.grasp.grasp_pose.orientation.y = 0;
		 place_goal.grasp.grasp_pose.orientation.z = 0;
		 place_goal.grasp.grasp_pose.orientation.w = 1;
		 place_goal.grasp.grasp_pose.position.x = 0;
		 place_goal.grasp.grasp_pose.position.y = 0;
		 place_goal.grasp.grasp_pose.position.z = 0;

		 //use the arm to place
		 place_goal.arm_name = "arm";
		 //padding used when determining if the requested place location
		 //would bring the object in collision with the environment
		 place_goal.place_padding = 0.02;
		 //how much the gripper should retreat after placing the object
		 place_goal.desired_retreat_distance = 0.06;
		 place_goal.min_retreat_distance = 0.05;
		 //we will be putting down the object along the "vertical" direction
		 //which is along the z axis in the base_link frame
		 geometry_msgs::Vector3Stamped direction;
		 direction.header.stamp = ros::Time::now();
		 direction.header.frame_id = KATANA_BASE_LINK;
		 direction.vector.x = 0;
		 direction.vector.y = 0;
		 direction.vector.z = -1;
		 place_goal.approach.direction = direction;
		 //request a vertical put down motion of 10cm before placing the object
		 place_goal.approach.desired_distance = 0.06;
		 place_goal.approach.min_distance = 0.05;
		 //we are not using tactile based placing
		 place_goal.use_reactive_place = false;
		 //send the goal
		 place_client->sendGoal(place_goal);
		 while (!place_client->waitForResult(ros::Duration(10.0))) {
		 ROS_INFO("Waiting for the place action...");
		 if (!nh.ok())
		 return -1;
		 }
		 object_manipulation_msgs::PlaceResult place_result =
		 *(place_client->getResult());
		 if (place_client->getState()
		 != actionlib::SimpleClientGoalState::SUCCEEDED) {
		 if (place_result.manipulation_result.value
		 == object_manipulation_msgs::ManipulationResult::RETREAT_FAILED) {
		 ROS_WARN(
		 "Place failed with error RETREAT_FAILED, ignoring! This may lead to collision with the object we just placed!");
		 } else {
		 ROS_ERROR(
		 "Place failed with error code %d", place_result.manipulation_result.value);
		 return -1;
		 }

		 }

		 //success!
		 ROS_INFO("Success! Object moved.");

		 move_arm_out_of_the_way();

		 object_in_gripper = 0;
		 */

		return true;
	}
示例#10
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;

	}
示例#11
0
	Pick_and_place_app(ros::NodeHandle *_nh) {

		nh = *_nh;

		//get parameters from parameter server
		nh.param<int>("sim", sim, 0);

		nh.param<int>("DIRECTLY_SELECT_GRASP_POSE", DIRECTLY_SELECT_GRASP_POSE,
				1);

		//the distance between the surface of the object to grasp and the GRIPPER_FRAME origin
		nh.param<double>("OBJECT_GRIPPER_STANDOFF", STANDOFF, 0.08);

		nh.param<std::string>("ARM_BASE_LINK", ARM_BASE_LINK,
				"katana_base_link");

		nh.param<std::string>("BASE_LINK", BASE_LINK, "base_link");

		nh.param<std::string>("GRIPPER_FRAME", GRIPPER_FRAME,
				"katana_gripper_tool_frame");
		nh.param<std::string>("FINGER_JOINT", FINGER_JOINT,
				"katana_l_finger_joint");
		nh.param<std::string>("ARM_NAME", ARM_NAME, "arm");

		clicked = 0;

		object_in_gripper = 0;

		pcl_input_point_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());

		//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

		// create TF listener
		tf_listener = new tf::TransformListener();

		pub_collision_object = nh.advertise<moveit_msgs::CollisionObject>(
				"collision_object", 10);

		ros::WallDuration(1.0).sleep();

		group = new move_group_interface::MoveGroup(ARM_NAME);

		//group->setPoseReferenceFrame(BASE_LINK);

		//group->setPlanningTime(5.0);

		group->setGoalTolerance(0.01);
		group->setGoalOrientationTolerance(0.005);

		group->allowReplanning(true);

		group->setWorkspace(-0.5,-0.6,-0.3,0.6,0.6,1.5);

		//wait for get planning scene server

		while(!ros::service::waitForService (GET_PLANNING_SCENE_SERVICE_NAME, ros::Duration(2.0)) && nh.ok()){
			ROS_INFO("Waiting for get planning scene service to come up");
		}
		if (!nh.ok())
			exit(0);
		get_planning_scene_srv = nh.serviceClient<moveit_msgs::GetPlanningScene>(GET_PLANNING_SCENE_SERVICE_NAME, true);


		//wait for detection client
		while (!ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME,
				ros::Duration(2.0)) && nh.ok()) {
			ROS_INFO("Waiting for object detection service to come up");
		}
		if (!nh.ok())
			exit(0);
		object_detection_srv = nh.serviceClient<
				tabletop_object_detector::TabletopDetection>(
				OBJECT_DETECTION_SERVICE_NAME, true);


		//wait for collision map processing client
		while (!ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME,
				ros::Duration(2.0)) && nh.ok()) {
			ROS_INFO("Waiting for collision processing service to come up");
		}
		if (!nh.ok())
			exit(0);
		collision_processing_srv =
				nh.serviceClient<
						tabletop_collision_map_processing::TabletopCollisionMapProcessing>(
						COLLISION_PROCESSING_SERVICE_NAME, true);


		cluster_bounding_box2_3d_client_ = nh.serviceClient<
				object_manipulation_msgs::FindClusterBoundingBox2>(
				CLUSTER_BOUNDING_BOX2_3D_NAME, true);


		vis_marker_publisher = nh.advertise<visualization_msgs::Marker>(
					"pick_and_place_markers", 128);


		//Sets the kinects tilt angle

		set_kinect_ptu("kurtana_pitch_joint", 0.75);

		ROS_INFO("Kinect lined up.");

		move_arm_out_of_the_way();

	}