Esempio n. 1
0
bool Actuator::moveToPose(geometry_msgs::Pose goalPose, char armName){
	bool isPlanningSuccess;
	moveit::planning_interface::MoveGroup::Plan my_plan;
	rightArm.setStartState(*rightArm.getCurrentState());
	std::string planningArm = "left_gripper";
	if(armName == amazon::MoveToGoal::RIGHT_ARM){
		planningArm = "right_gripper";
	}
	else if(armName != amazon::MoveToGoal::LEFT_ARM){
		ROS_WARN("Unknown arm received. Using left arm");
	}
	rightArm.setPoseTarget(goalPose, planningArm.c_str());

	// call the planner to compute the plan and visualize it
	isPlanningSuccess = rightArm.plan(my_plan);

	/* *****************  FOR DEBUGGING ***************/
	ROS_INFO("Visualizing plan %s", isPlanningSuccess?"":"FAILED");

	if(isPlanningSuccess){
		//rightArm.execute(my_plan);
		return true;
	}
	else{
		return false;
	}
}
void moveArmsToHomePose(moveit::planning_interface::MoveGroup &arms)
{
  std::map<std::string, double> target;

  target["r_elbow_flex_joint"] = -2.12441;
  //  target["r_forearm_roll_joint"] = -1.4175;
  target["r_shoulder_lift_joint"] = 1.24853;
  target["r_shoulder_pan_joint"] = -1.5;
  //  target["r_upper_arm_roll_joint"] = -1.55669;
  //  target["r_wrist_flex_joint"] = -1.8417;
  //  target["r_wrist_roll_joint"] = 0.21436;

  arms.setJointValueTarget(target);

  while (!arms.move()){
    ROS_INFO("Right arm motion failed. Retrying.");
  }

  target.clear();

  target["l_elbow_flex_joint"] = -1.68339;
  target["l_forearm_roll_joint"] = -1.73434;
  target["l_shoulder_lift_joint"] = 1.24852;
  target["l_shoulder_pan_joint"] = 0.06024;
  target["l_upper_arm_roll_joint"] = 1.78907;
  target["l_wrist_flex_joint"] = -0.0962141;
  target["l_wrist_roll_joint"] = -0.0864407;

  arms.setJointValueTarget(target);

  while (!arms.move()){
    ROS_INFO("Left arm motion failed. Retrying.");
  }
}
Esempio n. 3
0
    void reset_arms(){
        ros::AsyncSpinner spinner(0);
        spinner.start();

        std::map<std::string,double> jointmap;
        jointmap.insert(std::pair<std::string,double>("left_s0", -0.08));
        jointmap.insert(std::pair<std::string,double>("left_s1", -1.0));
        jointmap.insert(std::pair<std::string,double>("left_e0", -1.19));
        jointmap.insert(std::pair<std::string,double>("left_e1", 1.94));
        jointmap.insert(std::pair<std::string,double>("left_w0", 0.67));
        jointmap.insert(std::pair<std::string,double>("left_w1", 1.03));
        jointmap.insert(std::pair<std::string,double>("left_w2", -0.50));
        jointmap.insert(std::pair<std::string,double>("right_s0", 0.08));
        jointmap.insert(std::pair<std::string,double>("right_s1", -1.0));
        jointmap.insert(std::pair<std::string,double>("right_e0", 1.19));
        jointmap.insert(std::pair<std::string,double>("right_e1", 1.94));
        jointmap.insert(std::pair<std::string,double>("right_w0", -0.67));
        jointmap.insert(std::pair<std::string,double>("right_w1", 1.03));
        jointmap.insert(std::pair<std::string,double>("right_w2", 0.50));

        both_arms.setJointValueTarget(jointmap);
        both_arms.move();

        spinner.stop();
    }
bool ServiceWout::planAndMove()
{
	if(!trigger){
		return false;
	}

	trigger = false;

	ROS_INFO("Setting target pose...");

	ROS_INFO("Reference frame: %s", group->getEndEffectorLink().c_str());

	group->setPoseTarget(stored_pose);

	ROS_INFO("Preparing to plan...");

	moveit::planning_interface::MoveGroup::Plan my_plan;
	group->plan(my_plan);

	sleep(10.0);

	ROS_INFO("Preparing to move...");
  
	group->move();

	return true;
}
Esempio n. 5
0
void HapticsPSM::compute_force_in_tip_frame(geometry_msgs::Wrench &wrench){
    rot_mat6wrt0.setRPY(group->getCurrentRPY().at(0),
                        group->getCurrentRPY().at(1),
                        group->getCurrentRPY().at(2));
    tf_vec3.setValue(wrench.force.x,wrench.force.y,wrench.force.z);
    tf_vec3 = rot_mat6wrt0.transpose() * tf_vec3;
    wrench.force.x = tf_vec3.getX();
    wrench.force.y = tf_vec3.getY();
    wrench.force.z = tf_vec3.getZ();
}
void place(moveit::planning_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::PlaceLocation> loc;

geometry_msgs::PoseStamped p;
p.header.frame_id = "base_footprint";
p.pose.position.x = 0.7;
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);
}
moveit_msgs::CollisionObject createCollisionBox(
		moveit::planning_interface::MoveGroup& group, std::string object_id,
		double size_x, double size_y, double size_z, double pos_x, double pos_y,
		double pos_z, double orientation_x, double orientation_y,
		double orientation_z, double orientation_w) {
	moveit_msgs::CollisionObject collision_object;
	collision_object.header.frame_id = group.getPlanningFrame();

	/* The id of the object is used to identify it. */
	collision_object.id = object_id;

	/* Define a box to add to the world. */
	shape_msgs::SolidPrimitive primitive;
	primitive.type = primitive.BOX;
	primitive.dimensions.resize(3);
	primitive.dimensions[0] = size_x;
	primitive.dimensions[1] = size_y;
	primitive.dimensions[2] = size_z;

	/* A pose for the box (specified relative to frame_id) */
	geometry_msgs::Pose box_pose;
	box_pose.orientation.x = orientation_x;
	box_pose.orientation.y = orientation_y;
	box_pose.orientation.z = orientation_z;
	box_pose.orientation.w = orientation_w;
	box_pose.position.x = pos_x;
	box_pose.position.y = pos_y;
	box_pose.position.z = pos_z;

	collision_object.primitives.push_back(primitive);
	collision_object.primitive_poses.push_back(box_pose);
	collision_object.operation = collision_object.ADD;

	return collision_object;
}
Esempio n. 8
0
bool HapticsPSM::check_collison(){

    coll_res.clear();
    psm_planning_scene->checkCollisionUnpadded(coll_req,coll_res,*group->getCurrentState(),
                                            psm_planning_scene->getAllowedCollisionMatrix());
    if (coll_res.collision)
       {
         //ROS_INFO("COLLIDING contact_point_count=%d",(int)coll_res.contact_count);

         if (coll_res.contact_count > 0)
         {
           std_msgs::ColorRGBA color;
           color.r = 1.0;
           color.g = 0.0;
           color.b = 0.2;
           color.a = 0.8;
           visualization_msgs::MarkerArray markers;
           collision_detection::getCollisionMarkersFromContacts(markers,
                                                                group->getPlanningFrame().c_str(),
                                                                coll_res.contacts,
                                                                color,
                                                                ros::Duration(), // remain until deleted
                                                                0.002);
           publishMarkers(markers);
           }
         return true;
         }
    else
      {
        //ROS_INFO("Not colliding");

        // delete the old collision point markers
        visualization_msgs::MarkerArray empty_marker_array;
        publishMarkers(empty_marker_array);
        return false;
      }

}
void pick(moveit::planning_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 tiltHead(moveit::planning_interface::MoveGroup &head, double value)
{
  head.setJointValueTarget("head_tilt_joint", value);

  head.move();
}
Esempio n. 11
0
void HapticsPSM::run_haptic_alg(){

    if(check_collison()){
        //Step 1:
        get_collision_normals_and_points(coll_res.contacts, coll_psm.cur_normal_arr, coll_psm.cur_contact_pnts_arr, coll_psm.insertion_depths);
        if (coll_psm.contact_cnts_prev != coll_res.contacts.size()){
            ROS_INFO("Number of Contact Points = %i", coll_res.contacts.size());
        }
        //Step 2:
        compute_average_position(coll_psm.cur_contact_pnts_arr, coll_psm.locked_position);
        compute_average_normal(coll_psm.cur_normal_arr, coll_psm.cur_normal);
        if(coll_psm._first_contact){
            ROS_INFO("First contact occured");
            tf::Vector3 diff, cur_pos;
            get_current_position(cur_pos);
            diff = coll_psm.locked_position - cur_pos;
            coll_psm.spr_radius = diff.length();
            ROS_INFO("SPR Radius Detected to be %f",coll_psm.spr_radius);
            coll_psm._first_contact = false;
            ROS_INFO("Normal is nx = %f ny = %f  nz = %f", coll_psm.cur_normal.getX(),coll_psm.cur_normal.getY(),coll_psm.cur_normal.getZ());
        }
        //Step 3:
        if(has_normal_changed(coll_psm.cur_normal, coll_psm.pre_normal)){
//            ROS_INFO("Normal has Changed nx = %f ny = %f  nz = %f", coll_psm.cur_normal.getX(),coll_psm.cur_normal.getY(),coll_psm.cur_normal.getZ());
//            ROS_INFO("Normal has Changed pnx = %f pny = %f  pnz = %f", coll_psm.pre_normal.getX(),coll_psm.pre_normal.getY(),coll_psm.pre_normal.getZ());
        }
        //Step 5:
        compute_total_deflection(coll_psm.def_total);
        //Step 6:
        compute_deflection_along_normal(coll_psm.cur_normal, coll_psm.def_total, coll_psm.def_along_n);
        //Step 7:
        compute_deflection_force(spr_haptic_force.wrench,coll_psm.def_along_n);
        //Step 8:
        compute_force_in_tip_frame(spr_haptic_force.wrench);
        //Step 9:
        coll_psm.pre_normal = coll_psm.cur_normal;
        coll_psm.contact_cnts_prev = coll_res.contacts.size();
        if (coll_psm.cur_normal.angle(coll_psm.def_along_n) > coll_psm.epsilon){
        ROS_INFO("Angle between normal and deflection force %f", coll_psm.cur_normal.angle(coll_psm.def_along_n));
        }
        mesh_normal.wrench.force.x = coll_psm.cur_normal.getX();
        mesh_normal.wrench.force.y = coll_psm.cur_normal.getY();
        mesh_normal.wrench.force.z = coll_psm.cur_normal.getZ();

        normal_pub.publish(mesh_normal);
    }
    else{
        if(!coll_psm._first_contact){
            ROS_INFO("First contact Released");
            coll_psm._first_contact = true;
        }
        coll_psm.def_along_n.setValue(0,0,0);
        coll_psm.cur_normal.setValue(0,0,0);
        spr_haptic_force.wrench.force.x=0;
        spr_haptic_force.wrench.force.y=0;
        spr_haptic_force.wrench.force.z=0;
    }
    haptic_deflection.x = coll_psm.def_along_n.getX();
    haptic_deflection.y = coll_psm.def_along_n.getY();
    haptic_deflection.z = coll_psm.def_along_n.getZ();
    haptic_deflection_pub.publish(haptic_deflection);
    spr_haptic_pub.publish(spr_haptic_force);
    pose_pub.publish(group->getCurrentPose());


}
Esempio n. 12
0
void HapticsPSM::get_current_position(tf::Vector3 &v){
    v.setValue(group->getCurrentPose().pose.position.x,
               group->getCurrentPose().pose.position.y,
               group->getCurrentPose().pose.position.z);
}
Esempio n. 13
0
void Actuator::executeMotion(const amazon::MoveToGoalConstPtr& goal){

	if(goal->moveAction == amazon::MoveToGoal::MOVE_TO_POSE){
		ROS_INFO("Moving arm to specified location");
		if(moveToPose(goal->movePose, goal->arm))
			moveToServer.setSucceeded();
		else
			moveToServer.setAborted();
	}
	else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_PICK){
		// Do some error handling
		if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
			ROS_WARN("Incorrect shelf name passed in");
			moveToServer.setAborted();
		}
		else if(!isInsideShelf(goal->movePose.position, goal->shelf.data.at(0))){
			ROS_WARN("Point is not inside specified shelf");
			moveToServer.setAborted();
		}
		else{
			ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());

			geometry_msgs::Pose shelfPose;
			shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
			// make orientation horizontal in front of shelf
			shelfPose.orientation.w = 0.7071;
			shelfPose.orientation.y = 0.7071;
			// move to point in front of shelf, if successful, move into shelf
			if(moveToPose(shelfPose, goal->arm) && pushGrip(goal->arm, goal->shelf.data.at(0), goal->movePose.position)){// && moveToPose(goal->movePose, goal->arm)){
				moveToServer.setSucceeded();
			}
			else
				moveToServer.setAborted();
		}
	}
	else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_SHELF){
		// Do some error handling
		if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
			ROS_WARN("Incorrect shelf name passed in");
			moveToServer.setAborted();
		}
		else{
			ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());

			geometry_msgs::Pose shelfPose;
			shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
			// make orientation horizontal in front of shelf
			shelfPose.orientation.w = 0.7071;
			shelfPose.orientation.y = 0.7071;
			// move to point in front of shelf, if successful, move into shelf
			if(moveToPose(shelfPose, goal->arm)){
				moveToServer.setSucceeded();
			}
			else
				moveToServer.setAborted();
		}
	}
	else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_DROP){
		// Do some error handling
		if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
			ROS_WARN("Incorrect shelf name passed in");
			moveToServer.setAborted();
		}
		else if(!isInsideShelf(rightArm.getCurrentPose().pose.position, goal->shelf.data.at(0))){
			ROS_WARN("Robot is not inside specified shelf");
			moveToServer.setAborted();
		}
		else{
			ROS_INFO("Moving arm to front of shelf %s and then drop location", goal->shelf.data.c_str());

			geometry_msgs::Pose shelfPose;
			shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
			// make orientation horizontal in front of shelf
			shelfPose.orientation.w = 0.7071;
			shelfPose.orientation.y = 0.7071;
			// move to point in front of shelf, if successful, move into shelf
			if(moveToPose(shelfPose, goal->arm) && moveToPose(goal->movePose, goal->arm)){
				moveToServer.setSucceeded();
			}
			else
				moveToServer.setAborted();
		}
	}
	else if (goal->moveAction == amazon::MoveToGoal::SCOOP_SHELF){
		// Do some error handling
		if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
			ROS_WARN("Incorrect shelf name passed in");
			moveToServer.setAborted();
		}
		else{
			ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());

			geometry_msgs::Pose shelfPose;
			shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
			shelfPose.position.x -= SCOOP_LENGTH;
			// make orientation the same as the slope of shelf
			shelfPose.orientation.w = 0.4777;
			shelfPose.orientation.x = -0.5214;
			shelfPose.orientation.y = 0.5214;
			shelfPose.orientation.z = -0.4777;
			// move to point in front of shelf, if successful, move into shelf
			if(moveToPose(shelfPose, goal->arm)){
				moveToServer.setSucceeded();
			}
			else
				moveToServer.setAborted();
		}
	}
}
Esempio n. 14
0
    void wave()
    {
        ros::AsyncSpinner spinner(0);
        spinner.start();
        // reset_arms();
        sleep(1);
        //Create a series of stored plans, to be executed one by one in a blocking queue
        //Get current state of robot->create plan to next state->set new start state to that one->repeat
        robot_state::RobotStatePtr curr_state=right_arm.getCurrentState();
        
        std::map<std::string, double> r_jm;
        r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
        r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
        r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
        r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
        r_jm.insert(std::pair<std::string,double>("right_w0", -0.67));
        r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
        r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));

        moveit::planning_interface::MoveGroup::Plan r_ap0;
        right_arm.setJointValueTarget(r_jm);+
        right_arm.plan(r_ap0);

        curr_state->setStateValues(r_jm);
        right_arm.setStartState(*curr_state);

        r_jm.clear();
        r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
        r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
        r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
        r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
        r_jm.insert(std::pair<std::string,double>("right_w0", -1.5467));
        r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
        r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));
        
        moveit::planning_interface::MoveGroup::Plan r_ap1;
        right_arm.setJointValueTarget(r_jm);
        right_arm.plan(r_ap1);

        curr_state->setStateValues(r_jm);
        right_arm.setStartState(*curr_state);

        r_jm.clear();
        r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
        r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
        r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
        r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
        r_jm.insert(std::pair<std::string,double>("right_w0", -0.2062));
        r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
        r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));

        moveit::planning_interface::MoveGroup::Plan r_ap2;
        right_arm.setJointValueTarget(r_jm);
        right_arm.plan(r_ap2);

        curr_state->setStateValues(r_jm);
        right_arm.setStartState(*curr_state);

        r_jm.clear();
        r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
        r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
        r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
        r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
        r_jm.insert(std::pair<std::string,double>("right_w0", -1.5467));
        r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
        r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));

        moveit::planning_interface::MoveGroup::Plan r_ap3;
        right_arm.setJointValueTarget(r_jm);
        right_arm.plan(r_ap3);

        // curr_state->setStateValues(r_jm);
        // right_arm.setStartState(*curr_state);

        // r_jm.clear();
        // r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
        // r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
        // r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
        // r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
        // r_jm.insert(std::pair<std::string,double>("right_w0", -0.2062));
        // r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
        // r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));

        // moveit::planning_interface::MoveGroup::Plan r_ap4;
        // right_arm.setJointValueTarget(r_jm);
        // right_arm.plan(r_ap4);

        right_arm.execute(r_ap0);
        right_arm.execute(r_ap1);
        right_arm.execute(r_ap2);
        right_arm.execute(r_ap3);
        // right_arm.execute(r_ap4);

        spinner.stop();

    }
Esempio n. 15
0
  void leapmotionCallback(const leap_motion::leapros2::ConstPtr& dataHand)
{
      dataHand_=(*dataHand);
      
      
      //ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
      
      // Both limits for x,y,z to avoid small changes
      Updifferencex=dataLastHand_.palmpos.x+0.2;
      Downdifferencex=dataLastHand_.palmpos.x-0.2;
      Updifferencez=dataLastHand_.palmpos.z+0.3;
      Downdifferencez=dataLastHand_.palmpos.z-0.2;
      Updifferencey=dataLastHand_.palmpos.y+0.5;
      Downdifferencey=dataLastHand_.palmpos.y-0.5;
      //joint_msg_leap.header.stamp = ros::Time::now();
      
      if ((dataHand_.palmpos.x<Downdifferencex)||(dataHand_.palmpos.x>Updifferencex)||(dataHand_.palmpos.y<Downdifferencey)||(dataHand_.palmpos.y>Updifferencey)||(dataHand_.palmpos.z<Downdifferencez)||(dataHand_.palmpos.z>Updifferencez))
      {
      ros::AsyncSpinner spinner(1);
      spinner.start();
      //target_pose2.position.y +=(dataHand_.palmpos.x-dataLastHand_.palmpos.x)/100 ;
      target_pose2.position.z +=(dataHand_.palmpos.y-dataLastHand_.palmpos.y)/100 ;
      //target_pose2.position.x +=-(dataHand_.palmpos.z-dataLastHand_.palmpos.z)/100 ;
      if(target_pose2.position.z>Uplimitez)target_pose2.position.z=Uplimitez;
      if(target_pose2.position.z<Downlimitez)target_pose2.position.z=Downlimitez;
      //target_pose2.position.z += 0.2
      
      pgroup->setPoseTarget(target_pose2);
      bool success = pgroup->plan(*pplan);
      //pgroup->setStartStateToCurrentState();

      //we change the initial state
      //robot_state::RobotState start_state(*pgroup->getCurrentState());
      //ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
      //target_pose2.position.x=dataHand_.palmpos.x-dataLastHand_.palmpos.x;
      //target_pose2.position.y=dataHand_.palmpos.y-dataLastHand_.palmpos.y;
      //target_pose2.position.z=dataHand_.palmpos.z-dataLastHand_.palmpos.z;*/
      //ROS_INFO("Move ARM");
      //sleep(5.0);
    /*const robot_state::JointModelGroup *joint_model_group =
    start_state.getJointModelGroup(pgroup->getName());
    if(start_state.setFromIK(joint_model_group, target_pose2))
    {
    ROS_INFO("ik calculated");
    sleep (1);
    }/*
    

      pgroup->setStartState(start_state);
      spinner.stop();
      
      //group.setPoseTarget(target_pose2);
      /*moveit::planning_interface::MoveGroup::Plan my_plan;
      bool success = group.plan(my_plan);
      
      
      */
      }
      else
      {
      //printf("Palmpos \n X: %f\n  Y: %f\n Z: %f\n ",dataHand_.palmpos.x,dataHand_.palmpos.y,dataHand_.palmpos.z);
      }
      //save new value of the last position of the hand
       dataLastHand_=(*dataHand);

      //we print the position of all the hand
      
      //}

}
Esempio n. 16
0
//引用传参比较好,不改变值的情况下生明为const安全。
void add_object(const moveit::planning_interface::MoveGroup &group)
{
      ros::NodeHandle node_handle;
    //添加物体

    // Advertise the required topic
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    // Note that this topic may need to be remapped in the launch file
      ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
      while(planning_scene_diff_publisher.getNumSubscribers() < 1)
      {
        ros::WallDuration sleep_t(0.5);
        sleep_t.sleep();
      }

    // Define the attached object message
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    // We will use this message to add or
    // subtract the object from the world
    // and to attach the object to the robot
      moveit_msgs::AttachedCollisionObject attached_object;
      attached_object.link_name = "lLink7";
      /* The header must contain a valid TF frame*/
      attached_object.object.header.frame_id = group.getPlanningFrame();

      /* The id of the object */
      attached_object.object.id = "box1";

      /* A default left pose */
      geometry_msgs::Pose pose1;
      pose1.orientation.w = 1.0;
      pose1.position.x=0.4;
      pose1.position.y=0.64;
      pose1.position.z=0.6;
      /* Define a left box to be attached */
      shape_msgs::SolidPrimitive primitive1;
      primitive1.type = primitive1.BOX;
      primitive1.dimensions.resize(3);
      primitive1.dimensions[0] = 0.03;
      primitive1.dimensions[1] = 0.03;
      primitive1.dimensions[2] = 0.2;

      /* A default right pose */
      geometry_msgs::Pose pose2;
      pose2.orientation.w = 1.0;
      pose2.position.x=-0.4;
      pose2.position.y=0.7;
      pose2.position.z=0.4;
      /* Define a right box to be attached */
      shape_msgs::SolidPrimitive primitive2;
      primitive2.type = primitive2.BOX;
      primitive2.dimensions.resize(3);
      primitive2.dimensions[0] = 0.3;
      primitive2.dimensions[1] = 0.3;
      primitive2.dimensions[2] = 0.4;

      //容器使用push_back进行添加元素
      attached_object.object.primitives.push_back(primitive1);
      attached_object.object.primitive_poses.push_back(pose1);
      attached_object.object.primitives.push_back(primitive2);
      attached_object.object.primitive_poses.push_back(pose2);

    // Note that attaching an object to the robot requires
    // the corresponding operation to be specified as an ADD operation
      attached_object.object.operation = attached_object.object.ADD;


    // Add an object into the environment
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    // Add the object into the environment by adding it to
    // the set of collision objects in the "world" part of the
    // planning scene. Note that we are using only the "object"
    // field of the attached_object message here.
      ROS_INFO("Adding the object into the world ");
      moveit_msgs::PlanningScene planning_scene;
      planning_scene.world.collision_objects.push_back(attached_object.object);
      planning_scene.is_diff = true;
      planning_scene_diff_publisher.publish(planning_scene);
      sleep(2);
}