void DBaseDlg::loadModelButton_clicked(){ if (mCurrentLoadedModel) { //remove the previously loaded model, but don't delete it graspItGUI->getIVmgr()->getWorld()->destroyElement(mCurrentLoadedModel->getGraspableBody(), false); mCurrentLoadedModel = NULL; } if(mModelList.empty()){ DBGA("No model loaded..."); return; } //check out the model in the modelList GraspitDBModel* model = dynamic_cast<GraspitDBModel*>(mModelList[mModelMap[modelsComboBox->currentText().toStdString()]]); if(!model){ DBGA("Cannot recognize the model type"); return; } //check that this model is already loaded into Graspit, if not, load it if (!model->geometryLoaded()) { //this loads the actual geometry in the scene graph of the object if ( model->load(graspItGUI->getIVmgr()->getWorld()) != SUCCESS) { DBGA("Model load failed"); return; } } //adds the object to the collision detection system model->getGraspableBody()->addToIvc(); //todo: where to dynamic information come from? //model->getGraspableBody()->initDynamics(); //this adds the object to the graspit world so that we can see it graspItGUI->getIVmgr()->getWorld()->addBody(model->getGraspableBody()); //and remember it mCurrentLoadedModel = model; //model->getGraspableBody()->showAxes(false); model->getGraspableBody()->setTransparency(0.0); model->getGraspableBody()->setTran(transf::IDENTITY); graspsGroup->setEnabled(FALSE); //delete the previously loaded grasps deleteVectorElements<db_planner::Grasp*, GraspitDBGrasp*>(mGraspList); mGraspList.clear(); //initialize the grasp information for the new model initializeGraspInfo(); }
/* show the 3D object, if isNbr is true, we will show the neighbor after applying the alignment to the neighbor which transforms it to the current model's coordinate system */ void DBasePlannerDlg::show3DObject(bool isNbr) { db_planner::Model *m = mPlanningModel; if (isNbr) { GraspitDBModel *dbm = static_cast<GraspitDBModel *>(mNeighbors[neighborComboBox->currentIndex()].first); // load in the test model if (!dbm->geometryLoaded()) { //this loads the actual geometry in the scene graph of the object static_cast<GraspitDBModel *>(dbm)->load(mHand->getWorld()); } m = dbm; } if (m == mModelShown) { return; } if (isNbr) { //make necessary transformation on the model to be shown transf tr = transf::IDENTITY; float elmts[16]; if (mAligner->Align(*m, *mPlanningModel, elmts)) { mat3 mat; vec3 v(elmts[3], elmts[7], elmts[11]); mat(0) = elmts[0]; mat(1) = elmts[1]; mat(2) = elmts[2]; mat(3) = elmts[4]; mat(4) = elmts[5]; mat(5) = elmts[6]; mat(6) = elmts[8]; mat(7) = elmts[9]; mat(8) = elmts[10]; tr.set(mat, v); } static_cast<GraspitDBModel *>(m)->getGraspableBody()->setTran(tr); } //destroy the current one, not delete from the memory mHand->getWorld()->destroyElement(static_cast<GraspitDBModel *>(mModelShown)->getGraspableBody(), false); static_cast<GraspitDBModel *>(m)->getGraspableBody()->addToIvc(); //todo: where does dynamic information come from? //static_cast<GraspitDBModel*>(m)->getGraspableBody()->initDynamics(); //this adds the object to the graspit world mHand->getWorld()->addBody(static_cast<GraspitDBModel *>(m)->getGraspableBody()); mModelShown = m; }
void CGDBGraspProcessor::run() { std::vector<db_planner::Model*> models; //get the old-style for which the McGrip grasps work mDbMgr->ModelList(&models, db_planner::FilterList::OLD_STYLE_IV); std::vector<db_planner::Model*>::iterator it; GraspableBody *currentModel = NULL; //reset the stats and the results mProcessor->reset(); for (it=models.begin(); it!=models.end(); it++) { if (mMaxGrasps >=0 && mProcessedGrasps >= mMaxGrasps) break; //delete the previous model; careful, don't delete it if (currentModel) { mHand->getWorld()->destroyElement(currentModel, false); currentModel = NULL; } GraspitDBModel *gModel = dynamic_cast<GraspitDBModel*>(*it); if (!gModel) { assert(0); continue; } //load the current model if (!gModel->geometryLoaded()) { gModel->load(mHand->getWorld()); } gModel->getGraspableBody()->addToIvc(); mHand->getWorld()->addBody(gModel->getGraspableBody()); currentModel = gModel->getGraspableBody(); //get all the grasps std::vector<db_planner::Grasp*> grasps; if(!mDbMgr->GetGrasps(*gModel,mHand->getDBName().toStdString(), &grasps)){ DBGP("Load grasps failed - no grasps found for model " << gModel->ModelName()); continue; } //keep the human ones std::vector<db_planner::Grasp*>::iterator it2 = grasps.begin(); while (it2!=grasps.end()) { if( QString((*it2)->GetSource().c_str()) == "HUMAN_REFINED") { it2++; } else { delete (*it2); it2 = grasps.erase(it2); } } //and process them processGrasps(&grasps); //clean up while (!grasps.empty()) { delete grasps.back(); grasps.pop_back(); } } mProcessor->finalize(); if (currentModel) { mHand->getWorld()->destroyElement(currentModel, false); } while(!models.empty()) { DBGA("Deleting model"); delete models.back(); DBGA("Model deleted"); models.pop_back(); } }
/*Load the appropriate hand and graspable body into the world from the task record */ bool GraspAnalyzingTask::getGrasps(){ DBGA("Task ID " << mRecord.taskId << "- Model " << mRecord.model->ModelName()); //put the hand in the world if necessary: 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(mRecord.handName.c_str())) { DBGA("Grasp Planning Task: using currently loaded hand"); mHand = world->getCurrentHand(); } else { mHand = GraspitDBGrasp::loadHandFromDBName(QString(mRecord.handName.c_str())); if ( !mHand || !mDBMgr->setHand(mHand)) { DBGA("Failed to load hand"); mStatus = ERROR; return false; } } /* FIXME: if there is anything in contact with the hand, we should probably remove it here */ mDBMgr->setHand(mHand); vector<string> taskNameList; mDBMgr->TaskTypeList(&taskNameList); //see if this task is performed only on a single grasp -- assume grasps are sorted by epsilon quality unsigned int graspPos = taskNameList[mRecord.taskType-1].find("GRASP_NUM_"); //get grasps from dbmgr mDBMgr->GetGrasps(*mRecord.model, mRecord.handName, &mGraspList); //sort by epsilon quality sort(mGraspList.begin(), mGraspList.end(), compareGraspEpsilonQM); //if we are only using one grasp, kick all of the others out of the list if( graspPos!= string::npos){ graspNum = QString(taskNameList[mRecord.taskType-1].substr(graspPos+10, 1).c_str()).toInt(); DBGA("Grasp Number:"<< graspNum); if (graspNum >= mGraspList.size()){ DBGA("Grasp list is smaller than number of grasp requested! Size = : " << mGraspList.size()); mStatus = ERROR; return false; } for(unsigned int i = 0; i < mGraspList.size(); ++i){ if (i == graspNum) continue; delete mGraspList[i]; } mGraspList[0] = mGraspList[graspNum]; mGraspList.resize(1); } std::cout << "Grasp ID " << mGraspList[0]->GraspId(); //get the approach type if the grasp type specifies it unsigned int aTypePos = taskNameList[mRecord.taskType-1].find("APPROACH_TYPE_"); if( aTypePos!= string::npos){ mApproachType = ApproachType(QString(taskNameList[mRecord.taskType-1].substr(aTypePos+14,1).c_str()).toInt()); DBGA("Approach Type " << mApproachType << " QString Value " << taskNameList[mRecord.taskType-1].substr(aTypePos+14,1).c_str()); } else mApproachType = BY_MAGIC; //load the model into graspit GraspitDBModel* model = dynamic_cast<GraspitDBModel*>(mRecord.model); //check that this model is already loaded into Graspit, if not, load it if (!model->geometryLoaded()) { //this loads the actual geometry in the scene graph of the object if ( model->load(graspItGUI->getIVmgr()->getWorld()) != SUCCESS) { DBGA("GraspAnalyzingTask::getGrasp: Fatal Error Model load failed"); return false; } } bool ok; LimitFlags = QString(mRecord.misc.c_str()).toInt(&ok); if (!ok) LimitFlags = -1; mObject = model->getGraspableBody(); mObject->addToIvc(); graspItGUI->getIVmgr()->getWorld()->addBody(model->getGraspableBody()); if(getenv("GRASPIT_WAIT_DISPLAY_LEN")) waitLength = QString(getenv("GRASPIT_WAIT_DISPLAY_LEN")).toInt(); mHand->setRenderGeometry(waitLength); mObject->setRenderGeometry(waitLength); return true; }