Example #1
0
int main(int argc, char** argv) {
	const string wcFile = "Kr16WallWorkCell/Scene.wc.xml";
	const string deviceName = "KukaKr16";
	cout << "Trying to use workcell " << wcFile << " and device " << deviceName << endl;

	WorkCell::Ptr wc = WorkCellLoader::Factory::load(wcFile);
	Device::Ptr device = wc->findDevice(deviceName);
	if (device == NULL) {
		cerr << "Device: " << deviceName << " not found!" << endl;
		return 0;
	}
	
	const char* stats_str;
	double extend;
	
	if (argc > 1) {
		stats_str = argv[1];
		extend = atof(argv[2]);
	}else {
		stats_str = "stat.txt";
		extend = 0.1;
	}
	cout << extend << endl << stats_str <<endl;
	
	
	Math::seed();
	
	State state = wc->getDefaultState();
	
	Q start(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573);
	device->setQ(start,state);
	
	MovableFrame* bottle = wc->findFrame<MovableFrame>("Bottle");
	Kinematics::gripFrame(bottle,device->getEnd(),state);

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

	/** Most easy way: uses default parameters based on given device
		sampler: QSampler::makeUniform(device)
		metric: PlannerUtil::normalizingInfinityMetric(device->getBounds())
		extend: 0.05 */
	//QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, device, RRTPlanner::RRTConnect);

	/** More complex way: allows more detailed definition of parameters and methods */
	QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(device),constraint.getQConstraintPtr());
	QMetric::Ptr metric = MetricFactory::makeEuclidean<Q>();
	//double extend = 0.1;

	Q from(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573);
	//Q to(6,1.7,0.6,-0.8,0.3,0.7,-0.5); // Very difficult for planner
	Q to(6,1.571,0.006,0.030,0.153,0.762,4.490);

	if (!checkCollisions(device, state, detector, from))
		return 0;
	if (!checkCollisions(device, state, detector, to))
		return 0;

	cout << "Planning from " << from << " to " << to << endl;
	ofstream stat;
	stat.open(stats_str);
	stat << "Path length,Time" << endl;
	QPath path;
	Timer t;
	
	while (extend <= 0.5) {
		QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, sampler, metric, extend, RRTPlanner::RRTConnect);
	
		t.resetAndResume();
		planner->query(from,to,path,MAXTIME);
		t.pause();
		stat << path.size() << "," << t.getTime() << "," << extend << endl;
	
		for (int x=1; x<100; x++) {
			QPath path2;
			t.resetAndResume();
			planner->query(from,to,path2,MAXTIME);
			t.pause();
			stat << path2.size() << "," << t.getTime() << "," << extend << endl;
			if (path2.size() < path.size()) {
				path = path2;	
			}
		}
		extend += 0.01;
	}
	
	if (t.getTime() >= MAXTIME) {
		cout << "Notice: max time of " << MAXTIME << " seconds reached." << endl;
	}

	ofstream LUA; //create file object
	LUA.open("LUA.txt"); //open the file
	//write the LUA "header"
	LUA << "wc = rws.getRobWorkStudio():getWorkCell()\n";
	LUA << "state = wc:getDefaultState() \ndevice = wc:findDevice(\"KukaKr16\")\n";
	LUA << "bottle = wc:findFrame(\"Bottle\")\n";
	LUA << "function setQ(q)\n";
	LUA << "	qq = rw.Q(#q,q[1],q[2],q[3],q[4],q[5],q[6])\n";
	LUA << "	device:setQ(qq,state)\n";
	LUA << "	rws.getRobWorkStudio():setState(state)\n";
	LUA << "	rw.sleep(0.1)\n";
	LUA << "end\n";
	LUA << "setQ({-3.142, -0.827, -3.002, -3.143, 0.099, -1.573})\n";
	LUA << "rw.gripFrame(bottle,device:getEnd(),state)\n"; 
	
	for (QPath::iterator it = path.begin(); it < path.end(); it++) {
		//cout << *it << endl; //output path to Terminal
		ostringstream out; 
		out << *it; //write path to ostring
		string out2 = out.str(); //convert to string
		LUA << "setQ(" << out2.substr(4,out2.length()) << ")" << endl; // write path to LUA file.
	}
	
	LUA.close(); //close file

	cout << "Program done." << endl;
	return 0;
}
Example #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;
}