Beispiel #1
0
int main(int argc, char** argv) {
	//get wcFile and device name
	string wcFile = "/home/sdsuvei/robwork/RobWorkStudio/scenes/COBLab/KitchenScene.wc.xml";
	string robotName = "UR";
	string gripperName = "SDH";
	std::string connectorName = "URConnector";
	cout << "Trying to use workcell: " << wcFile << " and device " << robotName << endl;

	//load work cell
	WorkCell::Ptr wc = WorkCellFactory::load(wcFile);
	Device::Ptr UR = wc->findDevice(robotName);
	Device::Ptr gripper = wc->findDevice(gripperName);
	Device::Ptr connector = wc->findDevice(connectorName);
	if (UR == NULL) {
		cerr << "Device: " << robotName << " not found!" << endl;
		return 0;
	}

	//default state of the RobWork Workcell
	State state = wc->getDefaultState();

	//load the collision detector
	CollisionDetector cd(wc,ProximityStrategyFactory::makeDefaultCollisionStrategy());

	//ROS initialization
	ros::init(argc, argv, "listener");
	ros::NodeHandle nh;
	ros::Rate loop_rate(5);
	sub = nh.subscribe ("/joint_states", 1, jointCallback);

	while(ros::ok())
	{
		if(itHappened)
		{

	//RobWork robot model initialization
	//starting configuration of the UR5 arm
//	Q pos(6,0.0785, -1.7667, 2.037, -2.1101, -2.1724, -1.8505);
//	Q pos(6,-0.3946, -1.8944, 1.9378, -1.3807, -1.1023, -2.4439);
	Q pos(6, joint_pos[0], joint_pos[1], joint_pos[2], joint_pos[3], joint_pos[4], joint_pos[5]);
	cout<<"Initial arm configuration:\n"<<pos<<"\n";
	//set the initial position of the UR5 arm
	UR->setQ(pos,state);

	//starting configuration of the SDH gripper
	Q grip(7,-1.047,0.000,0.000,-0.765,-0.282,-0.846,0);
	//set the initial position of the gripper
	gripper->setQ(grip,state);

	//half-away configuration of the URconnector
	Q conn(1,-1.597);
	//set the half-away configuration of the URconnector
	connector->setQ(conn,state);

	//RobWork Workcell frame definitions
//	Frame* bottle = wc->findFrame("BottleTray"); //bootle frame
	MovableFrame* bottle = (MovableFrame*)wc->findFrame("BottleTray");
	Frame* marker=wc->findFrame("SDH.Marker"); //marker frame
	Frame* camera=wc->findFrame("Camera3D"); //camera frame
	Frame* pregrasp=wc->findFrame("Pregrasp"); // pregrasping frame

	if(cd.inCollision(state))
	{
		cout<<"Before change: In collision!\n";
	}

	//update the bottle frame with the value from the tracker
	Transform3D<> camTobj_rw=rw_camTobj(UR,state,bottle,camera);
	//change the object frame in the RobWork scene
	bottle->setTransform(camTobj_rw,state);

	if(cd.inCollision(state))
		{
			cout<<"After change: In collision!\n";
		}
	//store the kinematic values, from the RobWork workcell
	Transform3D<> kinematic_camTobj=Kinematics::frameTframe(camera,bottle,state);
	Transform3D<> kinematic_camTmrk=Kinematics::frameTframe(camera,marker,state);

    //compute the intermediate transforms
	// 1)URbase -> MARKER transform
	Transform3D<> urTmrk  = UR->baseTframe(marker,state);

    // 2)CAMERA -> OBJECT transform
	Transform3D<> camTobj=find_camTobj(UR,state,bottle,camera,kinematic_camTobj);

    // 3)CAMERA -> MARKER transform
	Transform3D<> camTmrk=find_camTmrk(UR,state,marker,camera,kinematic_camTmrk);

    // 4)MARKER -> OBJECT transform
	Transform3D<> mrkTobj=inverse(camTmrk)*camTobj;

	// 5)OBJECT -> PREGRASP transform
//	Transform3D<> objTpg=Kinematics::frameTframe(bottle,pregrasp,state);

	// 6)URbase -> PreGrasping frame
	Transform3D<> objTpg=Kinematics::frameTframe(bottle,pregrasp,state);
	Transform3D<> urTcam=find_urTcam(UR,state,camera);
	Transform3D<> urTpg=urTcam*camTobj_rw*objTpg; //take initial rotation of object as pregrasping rotation
//	Transform3D<> urTpg=urTcam*camTobj_rw;

	// 7)URbase -> OBJECT transform
	Transform3D<> urTobj((urTmrk*mrkTobj*objTpg).P(),urTpg.R());
//	Transform3D<> urTobj((urTmrk*mrkTobj).P(),urTpg.R());

	//determine initial distance between the gripper-marker and the object
	Vector3D<> dist=(urTobj.P()-urTmrk.P());
	cout<<"Initial distance is: "<<dist.norm2()<<endl;
	cout<<endl;
	//set the admissible threshold - distance between gripper-marker and object
	double tresh=0.01;

	//store the current value
	kinematic_camTmrk=camTmrk;
	kinematic_camTobj=camTobj;

	//start servoing method
	while (!cd.inCollision(state)){
		//compute current distance between gripper-marker and object
		Vector3D<> cur_dist=(urTobj.P()-urTmrk.P());
		cout<<"Current distance: "<<cur_dist.norm2()<<"\n";
		//stop program if threshold limit is exceeded
		if(cur_dist.norm2()<tresh)
		{
			cout<<"Threshold limit exceeded!"<<endl;
			ros::shutdown();
			break;
		}

		//compute the new urTmrk transform by performing interpolation between urTmrk & urTobj
		Transform3D<> urTmrk_new=computeinterpolation(urTmrk,urTobj,deltatess);

		//compute the new UR5 configuration which moves the gripper-marker closer to the object
		T1=clock();
		Q new_pos=computenewQ(pos,urTmrk,urTmrk_new,UR,state,marker,bottle); //new configuration
		//Bring robot in new configuration
		UR->setQ(new_pos,state); //set device
		cout<<"new_pos:"<<new_pos;
		cout<<endl;

		//store the new urTmrk transform
		urTmrk=urTmrk_new;
		//store the new UR5 arm configuration
		pos=new_pos;

		//recompute the intermediate transforms
	    // 1)CAMERA -> OBJECT transform
		Transform3D<> camTobj=find_camTobj(UR,state,bottle,camera,kinematic_camTobj);

	    // 2)CAMERA -> MARKER transform
		Transform3D<> camTmrk=find_camTmrk(UR,state,marker,camera,kinematic_camTmrk);

	    // 3)MARKER -> OBJECT transform
		Transform3D<> mrkTobj=inverse(camTmrk)*camTobj;

		// 4)URbase -> OBJECT transform
//		Transform3D<> urTobj((urTmrk*mrkTobj*objTpg).P(),urTpg.R()); //use the pregrasping rotation
		Transform3D<> urTobj((urTmrk*mrkTobj).P(),urTpg.R());
		//store kinematic values for camTobj & camTmrk
		kinematic_camTobj=camTobj;
		kinematic_camTmrk=camTmrk;

		//execute robot movement
		//open file to write the script
		std::ofstream myfile;
		cout << "Generating script ... ";
		myfile.open ("/home/sdsuvei/workspace/PythonScripts/armQ.py");
		if (myfile.is_open()){
			myfile << 	"#!/usr/bin/python \n \n" <<
						"import roslib \n" <<
						"roslib.load_manifest('cob_script_server') \n" <<
						"import rospy \n \n" <<
						"from simple_script_server import * \n" <<
						"sss = simple_script_server() \n \n" <<
						"if __name__ == \"__main__\": \n" <<
						"	rospy.init_node(\"asd\") \n" <<
						"	sss.move(\"arm\", [["<<pos[0]<<","<<pos[1]<<","<<pos[2]<<","<<pos[3]<<","<<pos[4]<<","<<pos[5]<<"]]) \n";
			myfile.close();
			cout << "Done!" << endl;
		}
		else{
			cout << "myfile not open!" << endl;
		}

		int val1;
		cout<<"\n Press key to allow movement: ";
		cin>>val1;
		cout<<"\n";

		cout << "Executing movement..." << endl;
		system("python /home/sdsuvei/workspace/PythonScripts/armQ.py");


		cout << "Movement done!" << endl;

		int val;
		cout<<"\n Press any key to continue ";
		cin>>val;
		cout<<"\n";
	}
	itHappened=false;
	}
	ros::spinOnce();
	loop_rate.sleep();
	}
	cout<<"After While";
			cout<<endl;

	if(flag)
		{
			cout<<"\nOrientation impossible! Try a different grasping approach!\n"<< endl;
		}
	else
		{
			cout<<"\nDone!\n\n";
		}
	return 0;
}
Beispiel #2
0
tuple<double, double, double> pathPlannerFunc(double extend){

    const string wcFile = "/home/student/ROVI/Mand2/Kr16WallWorkCell/Scene.wc.xml";
    const string deviceName = "KukaKr16";
    const string bottle = "Bottle";

    Q from(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573);
    Q to(6,1.571,0.006,0.030,0.153,0.762,4.490);

    WorkCell::Ptr wc = WorkCellLoader::Factory::load(wcFile);
    Device::Ptr device = wc->findDevice(deviceName);

    rw::kinematics::Frame *deviceB = wc->findFrame(bottle);

    if (device == NULL) {
        cerr << "Device: " << deviceName << " not found!" << endl;
        exit(-1);
    }
    if (deviceB == NULL) {
        cerr << "Device: " << bottle << " not found!" << endl;
        exit(-1);
    }

    rw::kinematics::State state = wc->getDefaultState();

    device->setQ(from, state);
    Kinematics::gripFrame(deviceB,device->getEnd(),state);

    CollisionDetector detector(wc, ProximityStrategyFactory::makeDefaultCollisionStrategy());
    PlannerConstraint constraint = PlannerConstraint::make(&detector,device,state);

    QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(device),constraint.getQConstraintPtr());
    QMetric::Ptr metric = MetricFactory::makeEuclidean<Q>();
    QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, sampler, metric, extend, RRTPlanner::RRTConnect);

    if (!checkCollisions(device, state, detector, from))
        exit(-1);
    if (!checkCollisions(device, state, detector, to))
        exit(-1);

    PathAnalyzer::CartesianAnalysis distance_traveled;
    double path_length = 0;

    PathAnalyzer path_analyse(device,state);

    QPath path;

    Timer t;
    t.resetAndResume();
    planner->query(from,to,path,MAXTIME);
    t.pause();
    distance_traveled = path_analyse.analyzeCartesian(path,deviceB);
    path_length += distance_traveled.length;

    cout << extend<< "\tPath of length " << path.size() << " found in " << t.getTime() << " seconds." << endl;
    if (t.getTime() >= MAXTIME) {
        cout << "Notice: max time of " << MAXTIME << " seconds reached." << endl;
    }

    for (QPath::iterator it = path.begin(); it < path.end(); it++) {
        cout << "set" << *it << endl;//"setQ" << bins.substr(3,bins.length()) << endl;
    }

    auto tmp = make_tuple(extend, path_length,t.getTime());

    return tmp;
}