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