void DBaseBatchPlanner::plannerComplete()
{
if (mPlanner->isActive()) {
DBGAF(mLogStream,"Planner is not finished!");
return;
}
//this prints the remainins solutions
plannerUpdate();
DBGAF(mLogStream,"Planner completed; starting shutdown");
if (mType == GRIPPER) {
graspitCore->getWorld()->destroyElement(mHand,false);
fprintf(stderr,"Taking scans...\n");
takeScans();
}
if (mResultFile!=stderr && mResultFile!=stdout) fclose(mResultFile);
//schedule the sensor to give us the exit signal in 3 seconds
//this should give the planner time to finish
mTimerSensor = new SoTimerSensor(sensorCB, this);
mTimerSensor->setInterval( SbTime( 3.0 ));
mTimerSensor->schedule();
}
void AutoGraspGenerationDlg::plannerInit_clicked()
{
QString s = plannerTypeBox->currentText();
if (s == QString("Sim. Ann.")) {
if (mPlanner) delete mPlanner;
mPlanner = new SimAnnPlanner(mHand);
((SimAnnPlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setEnabled(TRUE);
} else if (s == QString("Loop")) {
if (mPlanner) delete mPlanner;
mPlanner = new LoopPlanner(mHand);
((LoopPlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setEnabled(TRUE);
} else if (s == QString("Multi-Threaded")) {
if (mPlanner) delete mPlanner;
mPlanner = new GuidedPlanner(mHand);
((GuidedPlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setCurrentItem(2);
energyBox->setEnabled(FALSE);
} else if (s == QString("Online") ) {
if (mPlanner) delete mPlanner;
mPlanner = new OnLinePlanner(mHand);
((OnLinePlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setEnabled(TRUE);
energyBox->setCurrentItem(2);
QString n;
n.setNum(3000);
annStepsEdit->setText(n);
QObject::connect(mPlanner,SIGNAL(update()),this,SLOT(onlinePlannerUpdate()));
}
else if ( s == QString("Time Test") ) {
if (mPlanner) delete mPlanner;
mPlanner = new MTTester(mHand);
} else {
fprintf(stderr,"Unknown planner type requested\n");
return;
}
//graspItGUI->getIVmgr()->getWorld()->load("worlds/barrett_cordless_drill.xml");
graspItGUI->getIVmgr()->getWorld()->setCurrentPlanner(mPlanner);
QObject::connect(mPlanner,SIGNAL(update()),this,SLOT(plannerUpdate()));
QObject::connect(mPlanner,SIGNAL(complete()),this,SLOT(plannerComplete()));
updateStatus();
plannerReset_clicked();
}
void EigenGraspPlannerDlg::plannerInit_clicked()
{
QString s = plannerTypeBox->currentText();
if (s == QString("Sim. Ann.")) {
if (mPlanner) delete mPlanner;
mPlanner = new SimAnnPlanner(mHand);
((SimAnnPlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setEnabled(TRUE);
} else if (s == QString("Loop")) {
if (mPlanner) delete mPlanner;
mPlanner = new LoopPlanner(mHand);
((LoopPlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setEnabled(TRUE);
} else if (s == QString("Multi-Threaded")) {
if (mPlanner) delete mPlanner;
mPlanner = new GuidedPlanner(mHand);
((GuidedPlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setCurrentItem(2);
energyBox->setEnabled(FALSE);
} else if (s == QString("Online") ) {
if (mPlanner) delete mPlanner;
mPlanner = new OnLinePlanner(mHand);
((OnLinePlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setEnabled(TRUE);
energyBox->setCurrentItem(2);
QString n;
n.setNum(2000);
annStepsEdit->setText(n);
QObject::connect(mPlanner,SIGNAL(update()),this,SLOT(onlinePlannerUpdate()));
}else if (s == QString("Num-Contacts")) {
if (mPlanner) delete mPlanner;
mPlanner = new GuidedPlanner(mHand);
((GuidedPlanner*)mPlanner)->setModelState(mHandObjectState);
energyBox->setCurrentItem(0);
energyBox->setEnabled(FALSE);
} else if ( s == QString("Time Test") ) {
if (mPlanner) delete mPlanner;
mPlanner = new MTTester(mHand);
} else {
fprintf(stderr,"Unknown planner type requested\n");
return;
}
QObject::connect(mPlanner,SIGNAL(update()),this,SLOT(plannerUpdate()));
QObject::connect(mPlanner,SIGNAL(complete()),this,SLOT(plannerComplete()));
updateStatus();
plannerReset_clicked();
}
void GuidedGraspPlanningTask::start()
{
//get the details of the planning task itself
if (!mDBMgr->GetPlanningTaskRecord(mRecord.taskId, &mPlanningTask)) {
DBGA("Failed to get planning record for task");
mStatus = ERROR;
return;
}
World *world = graspItGUI->getIVmgr()->getWorld();
//check if the currently selected hand is the same as the one we need
//if not, load the hand we need
if (world->getCurrentHand() &&
GraspitDBGrasp::getHandDBName(world->getCurrentHand()) == QString(mPlanningTask.handName.c_str())) {
DBGA("Grasp Planning Task: using currently loaded hand");
mHand = world->getCurrentHand();
} else {
QString handPath = GraspitDBGrasp::getHandGraspitPath(QString(mPlanningTask.handName.c_str()));
handPath = QString(getenv("GRASPIT")) + handPath;
DBGA("Grasp Planning Task: loading hand from " << handPath.latin1());
mHand = static_cast<Hand*>(world->importRobot(handPath));
if ( !mHand ) {
DBGA("Failed to load hand");
mStatus = ERROR;
return;
}
}
//check for virtual contacts
if (mHand->getNumVirtualContacts()==0) {
DBGA("Specified hand does not have virtual contacts defined");
mStatus = ERROR;
return;
}
//load the object
GraspitDBModel *model = static_cast<GraspitDBModel*>(mPlanningTask.model);
if (model->load(world) != SUCCESS) {
DBGA("Grasp Planning Task: failed to load model");
mStatus = ERROR;
return;
}
mObject = model->getGraspableBody();
mObject->addToIvc();
world->addBody(mObject);
//initialize the planner
GraspPlanningState seed(mHand);
seed.setObject(mObject);
seed.setPositionType(SPACE_AXIS_ANGLE);
seed.setPostureType(POSE_EIGEN);
seed.setRefTran(mObject->getTran());
seed.reset();
mPlanner = new GuidedPlanner(mHand);
mPlanner->setModelState(&seed);
mPlanner->setContactType(CONTACT_PRESET);
if (mPlanningTask.taskTime >= 0) mPlanner->setMaxTime(mPlanningTask.taskTime);
else mPlanner->setMaxTime(-1);
QObject::connect(mPlanner, SIGNAL(update()), this, SLOT(plannerUpdate()));
QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete()));
if (!mPlanner->resetPlanner()) {
DBGA("Grasp Planning Task: failed to reset planner");
mStatus = ERROR;
return;
}
mLastSolution = 0;
mPlanner->startPlanner();
mStatus = RUNNING;
}
bool DBaseBatchPlanner::startPlanner()
{
if (!mHand || !mObject || mMaxTime <= 0) {
DBGA("Can not start batch planner; Hand or Object or Time not set");
return false;
}
GraspPlanningState seed(mHand);
seed.setObject(mObject);
seed.setPositionType(SPACE_AXIS_ANGLE);
seed.setPostureType(POSE_EIGEN);
seed.setRefTran(mObject->getTran());
seed.reset();
//sim ann planner can be used for testing
//mPlanner = new SimAnnPlanner(mHand);
if (mType == DEXTEROUS) {
//the real multi-threaded planner
mPlanner = new GuidedPlanner(mHand);
int numChildren;
//I think for best results setMaxChildren(# of CPU cores - 1)
#ifdef WIN32
QString numCPU = QString(getenv("NUMBER_OF_PROCESSORS"));
if (numCPU.isNull()) {
DBGAF(mLogStream,"NUMBER_OF_PROCESSORS env var. not specified; using 1 child thread");
numChildren = 1;
} else {
numChildren = numCPU.toInt();
}
if (numChildren <= 0) {
DBGAF(mLogStream,"Can not understand NUMBER_OF_PROCESSORS: " << numCPU.latin1());
}
#else
//on Linux we hard-code 2 CPU's for now
numChildren = 2;
#endif
if (numChildren <= 0) {
numChildren = 1;
} else {
numChildren --;
if (!numChildren) numChildren = 1;
}
//can be overriden here if you want to hard-code some other value in
DBGAF(mLogStream,"Using up to " << numChildren << " child threads");
((GuidedPlanner*)mPlanner)->setMaxChildren(numChildren);
} else if (mType == GRIPPER) {
//simple looping sim ann planner
mPlanner = new LoopPlanner(mHand);
mPlanner->setEnergyType(ENERGY_CONTACT);
}
mPlanner->setContactType(CONTACT_PRESET);
mPlanner->setMaxSteps(65000);
mPlanner->setRepeat(true);
mPlanner->setMaxTime(mMaxTime);
static_cast<SimAnnPlanner*>(mPlanner)->setModelState(&seed);
if (!mPlanner->resetPlanner()) {
DBGA("Failed to reset planner");
return false;
}
QObject::connect(mPlanner, SIGNAL(update()), this, SLOT(plannerUpdate()));
QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete()));
mPlanner->startPlanner();
return true;
}
