コード例 #1
0
ファイル: demo.cpp プロジェクト: mpomarlan/moveit_puzzle_demo
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);
}
コード例 #2
0
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
ファイル: pick_and_place.cpp プロジェクト: fivef/kate_apps
	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;
	}