int main(int argc, char **argv)
{
signal(SIGSEGV, handler);
signal(SIGABRT, handler);
PRINT_INIT_STD();
std::string worldFilename;
std::string robotFilename;
std::string objectFilename;
std::string outputDirectory;
bool saveSeparate;
Eigen::Vector3d objPos;
int maxPlanningSteps = 50000;
if (!loadParams(argc, argv, worldFilename, robotFilename, objectFilename, outputDirectory, saveSeparate, objPos, maxPlanningSteps))
{
PRINTERROR("Could not read arguments");
return 1;
}
PRINTMSG("Creating planner");
std::string name = "EigenGraspPlanner1"; // TODO make parameter
SHARED_PTR<GraspIt::GraspItSceneManager> graspitMgr(new GraspIt::GraspItSceneManagerHeadless());
#ifdef USE_EIGENGRASP_NOQT
SHARED_PTR<GraspIt::EigenGraspPlannerNoQt> p(new GraspIt::EigenGraspPlannerNoQt(name, graspitMgr));
#else
SHARED_PTR<GraspIt::EigenGraspPlanner> p(new GraspIt::EigenGraspPlanner(name, graspitMgr));
#endif
// TODO parameterize:
// Names for robot and object if not loaded from a world file.
// If loaded from a world file, will be overwritten.
std::string useRobotName="Robot1";
std::string useObjectName="Object1";
if (!worldFilename.empty())
{
PRINTMSG("Loading world");
graspitMgr->loadWorld(worldFilename);
std::vector<std::string> robs = graspitMgr->getRobotNames();
std::vector<std::string> objs = graspitMgr->getObjectNames(true);
if (robs.empty())
{
PRINTERROR("No robots loaded");
return 1;
}
if (objs.empty())
{
PRINTERROR("No graspable objects loaded");
return 1;
}
if (robs.size()!=1)
{
PRINTERROR("Exactly 1 robot should have been loaded");
return 1;
}
if (objs.size()!=1)
{
PRINTERROR("Exactly 1 graspable object should have been loaded");
return 1;
}
useRobotName=robs.front();
useObjectName=objs.front();
PRINTMSG("Using robot "<<useRobotName<<" and object "<<useObjectName);
}
else
{
// TODO add an option to set the transforms.
// For now, they're put in the origin. For the planning, this should not really matter...
GraspIt::EigenTransform robotTransform;
GraspIt::EigenTransform objectTransform;
robotTransform.setIdentity();
objectTransform.setIdentity();
objectTransform.translate(objPos);
// objectTransform.translate(Eigen::Vector3d(100,0,0));
std::string robotName(useRobotName);
std::string objectName(useObjectName);
if ((graspitMgr->loadRobot(robotFilename, robotName, robotTransform) != 0) ||
(graspitMgr->loadObject(objectFilename, objectName, true, objectTransform)))
{
PRINTERROR("Could not load robot or object");
return 1;
}
}
bool createDir = true;
bool saveIV = true;
bool forceWrite = createDir; // only enforce if creating dir is also allowed
// in case one wants to view the initial world before planning, save it:
graspitMgr->saveGraspItWorld(outputDirectory + "/startWorld.xml", createDir);
graspitMgr->saveInventorWorld(outputDirectory + "/startWorld.iv", createDir);
if (saveSeparate)
{
graspitMgr->saveRobotAsInventor(outputDirectory + "/robotStartpose.iv", useRobotName, createDir, forceWrite);
graspitMgr->saveObjectAsInventor(outputDirectory + "/object.iv", useObjectName, createDir, forceWrite);
}
int repeatPlanning = 1;
int keepMaxPlanningResults = 3;
bool finishWithAutograsp = false;
p->plan(maxPlanningSteps, repeatPlanning, keepMaxPlanningResults, finishWithAutograsp);
PRINTMSG("Saving results as world files");
bool saveWorld = true;
std::string resultsWorldDirectory = outputDirectory;
std::string filenamePrefix = "world";
p->saveResultsAsWorldFiles(resultsWorldDirectory, filenamePrefix, saveWorld, saveIV, createDir, saveSeparate);
std::vector<GraspIt::EigenGraspResult> allGrasps;
p->getResults(allGrasps);
PRINTMSG("Grasp results:");
std::vector<GraspIt::EigenGraspResult>::iterator it;
for (it = allGrasps.begin(); it != allGrasps.end(); ++it)
{
PRINTMSG(*it);
}
PRINTMSG("Quitting program.");
return 1;
}
int run(int argc, char **argv)
{
if (argc < 4)
{
PRINTMSG("Usage: " << argv[0] << " <outputDir> <robotFile> <objectFile>");
return 0;
}
std::string outputDirectory(argv[1]);
std::string robotFilename(argv[2]);
std::string objectFilename(argv[3]);
PRINTMSG("Creating database");
std::string name = "Database1";
SHARED_PTR<GraspIt::GraspItSceneManager> graspitMgr(new GraspIt::GraspItSceneManagerHeadless());
SHARED_PTR<GraspIt::GraspItSimpleDBManager> mgr(new GraspIt::GraspItSimpleDBManager(name, graspitMgr));
std::string robotName("Robot1");
std::string objectName("Object1");
PRINTMSG("Now loading robot");
int robotID = -1;
int objectID = -1;
// here we'd have to put the robot joint names, we'll leave this empty for this test fle
std::vector<std::string> jointNames;
if ((robotID = mgr->loadRobotToDatabase(robotFilename, robotName, jointNames)) < 0)
{
PRINTERROR("Could not load robot");
return 0;
}
PRINTMSG("Now loading object");
if ((objectID = mgr->loadObjectToDatabase(objectFilename, objectName, true)) < 0)
{
PRINTERROR("Could not load object");
return 0;
}
// Now objects should be in the database only, but not in the world.
// Re-add them to test if the world still workds afterwards:
PRINTMSG("Now loading robot to world");
if (mgr->loadToWorld(robotID, GraspIt::EigenTransform::Identity()) != 0)
{
PRINTERROR("Could not add the robot to the graspit world");
return 0;
}
PRINTMSG("Now loading object to world");
if (mgr->loadToWorld(objectID, GraspIt::EigenTransform::Identity()) != 0)
{
PRINTERROR("Could not add the object to the graspit world");
return 0;
}
PRINTMSG("Saving world files");
// test to see if it worked: save as world
bool createDir = true; // if true, the directory will be created, if it doesn't exist.
graspitMgr->saveGraspItWorld(outputDirectory + "/dbtest/world.xml", createDir);
graspitMgr->saveInventorWorld(outputDirectory + "/dbtest/world.iv", createDir);
PRINTMSG("Quitting program.");
return 0;
}
int main(int argc, char **argv)
{
signal(SIGSEGV, handler);
signal(SIGABRT, handler);
PRINT_INIT_STD();
std::string worldFilename;
std::string robotFilename;
std::string objectFilename;
std::string outputDirectory;
if (!loadParams(argc, argv, worldFilename, robotFilename, objectFilename, outputDirectory))
{
return 1;
}
PRINTMSG("Creating planner");
std::string name = "EigenGraspPlanner1"; // TODO make parameter
SHARED_PTR<GraspIt::GraspItSceneManager> graspitMgr(new GraspIt::GraspItSceneManagerNoGui());
#ifdef USE_EIGENGRASP_NOQT
SHARED_PTR<GraspIt::EigenGraspPlannerNoQt> p(new GraspIt::EigenGraspPlannerNoQt(name, graspitMgr));
#else
SHARED_PTR<GraspIt::EigenGraspPlanner> p(new GraspIt::EigenGraspPlanner(name, graspitMgr));
#endif
if (!worldFilename.empty())
{
PRINTMSG("Loading world");
graspitMgr->loadWorld(worldFilename);
}
else
{
// TODO add an option to set the transforms.
// For now, they're put in the origin. For the planning, this should not really matter...
GraspIt::EigenTransform robotTransform;
GraspIt::EigenTransform objectTransform;
robotTransform.setIdentity();
objectTransform.setIdentity();
// objectTransform.translate(Eigen::Vector3d(100,0,0));
std::string robotName("Robot1"); // TODO parameterize
std::string objectName("Object1");
if ((graspitMgr->loadRobot(robotFilename, robotName, robotTransform) != 0) ||
(graspitMgr->loadObject(objectFilename, objectName, true, objectTransform)))
{
PRINTERROR("Could not load robot or object");
return 1;
}
// in case one wants to view the initial world before planning, save it:
graspitMgr->saveGraspItWorld(outputDirectory + "/worlds/startWorld.xml");
graspitMgr->saveInventorWorld(outputDirectory + "/worlds/startWorld.iv");
}
// now save the world again as inventor file, to test
// p->saveIVWorld("test.iv");
int maxPlanningSteps = 50000;
int repeatPlanning = 1;
int keepMaxPlanningResults = 3;
bool finishWithAutograsp = false;
p->plan(maxPlanningSteps, repeatPlanning, keepMaxPlanningResults, finishWithAutograsp);
PRINTMSG("Saving results as world files");
bool createDir = true;
bool saveIV = true;
bool saveWorld = true;
std::string resultsWorldDirectory = outputDirectory;
std::string filenamePrefix = "world";
p->saveResultsAsWorldFiles(resultsWorldDirectory, filenamePrefix, saveWorld, saveIV, createDir);
std::vector<GraspIt::EigenGraspResult> allGrasps;
p->getResults(allGrasps);
PRINTMSG("Grasp results:");
std::vector<GraspIt::EigenGraspResult>::iterator it;
for (it = allGrasps.begin(); it != allGrasps.end(); ++it)
{
PRINTMSG(*it);
}
PRINTMSG("Quitting program.");
return 1;
}