diff --git a/VirtualRobot/examples/reachability/reachabilityScene.cpp b/VirtualRobot/examples/reachability/reachabilityScene.cpp index 9948acb522b438c022632e417a033e689d20667d..e7e72d7d07e02dd2ca5df2ede59b85d0eec10c8e 100644 --- a/VirtualRobot/examples/reachability/reachabilityScene.cpp +++ b/VirtualRobot/examples/reachability/reachabilityScene.cpp @@ -17,6 +17,11 @@ #include "reachabilityWindow.h" +// this flag can be used to build a good representation of the workspace +// the reachability data will be extended in endless mode and +// after every 1 mio-th update a snapshot is saved. +//#define ENDLESS + //#define ICUB //#define AXIS_X @@ -25,6 +30,58 @@ using std::cout; using std::endl; using namespace VirtualRobot; + + + +void endlessExtend(std::string robotFile, std::string reachFile) +{ + int steps = 1000000; + + VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robotFile); + VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(reachFile); + + // load robot + RobotPtr robot; + try + { + robot = RobotIO::loadRobot(robotFile); + } + catch (VirtualRobotException &e) + { + cout << " ERROR while creating robot" << endl; + cout << e.what(); + return; + } + + if (!robot) + { + cout << " ERROR while creating robot" << endl; + return; + } + + // load reach file + ReachabilityPtr reachSpace(new Reachability(robot)); + reachSpace->load(reachFile); + reachSpace->print(); + + time_t time_now = time(NULL); + struct tm * timeinfo; + timeinfo = localtime (&time_now); + char buffer [255]; + strftime (buffer,255,"ReachabilityData_ENDLESS_%Y_%m_%d__%H_%M_%S_",timeinfo); + int nr = 0; + while (true) + { + reachSpace->addRandomTCPPoses(steps); + reachSpace->print(); + std::stringstream ss; + ss << buffer << "_" << nr << ".bin"; + reachSpace->save(ss.str()); + nr++; + } + +} + int main(int argc, char *argv[]) { SoDB::init(); @@ -39,6 +96,7 @@ int main(int argc, char *argv[]) #else std::string filenameRob("robots/ArmarIII/ArmarIII.xml"); Eigen::Vector3f axisTCP(0,0,1.0f); + filenameReach = "reachability/ArmarIII_PlatformHipRightArm.bin"; #endif VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filenameRob); @@ -67,11 +125,13 @@ int main(int argc, char *argv[]) cout << "Using robot at " << filenameRob << endl; cout << "Using reachability file from " << filenameReach << endl; +#ifdef ENDLESS + endlessExtend(filenameRob,filenameReach); + return 0; +#endif reachabilityWindow rw(filenameRob,filenameReach,axisTCP); - rw.main(); return 0; - } diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp index a4cf26b28d1bcd7d800b61eddd099b3db9947a86..56954921b40d5ae6ad021508941767315e215449 100644 --- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp +++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp @@ -26,7 +26,6 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; -//#define ENDLESS reachabilityWindow::reachabilityWindow(std::string &sRobotFile, std::string &reachFile, Eigen::Vector3f &axisTCP, Qt::WFlags flags) :QMainWindow(NULL) @@ -366,7 +365,7 @@ void reachabilityWindow::extendReach() return; } int steps = UI.spinBoxExtend->value(); -#ifdef ENDLESS +/*#ifdef ENDLESS time_t time_now = time(NULL); struct tm * timeinfo; timeinfo = localtime (&time_now); @@ -383,7 +382,7 @@ void reachabilityWindow::extendReach() nr++; } -#endif +#endif*/ reachSpace->addRandomTCPPoses(steps); reachSpace->print(); }