Exemple #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;
	}
}
Exemple #2
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;
      }

}
Exemple #3
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();

    }