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;
}
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;
}
