void PreGraspCheckTask::loadObject()
{
World *world = graspitCore->getWorld();
GraspitDBModel *model = static_cast<GraspitDBModel *>(mPlanningTask.model);
if (model->load(world) != SUCCESS) {
DBGA("Grasp Planning Task: failed to load model");
mStatus = FAILED;
return;
}
mObject = model->getGraspableBody();
mObject->addToIvc();
world->addBody(mObject);
}
/* 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;
}
/*!
Load the object from the task record.
*/
void GraspPlanningTask::getObject(){
//load the model
GraspitDBModel *model = static_cast<GraspitDBModel*>(mRecord.model);
if (model->load(graspItGUI->getIVmgr()->getWorld()) != SUCCESS) {
//attempt repair
DBGA("Grasp Planning Task: failed to load model");
mStatus = ERROR;
return;
}
//Get the object from the model
mObject = model->getGraspableBody();
mObject->addToIvc();
//Add the body to the world
graspItGUI->getIVmgr()->getWorld()->addBody(mObject);
mObject->setMaterial(wood);
//Uncomment to add table obstacle to the world
// Body * table = graspItGUI->getIVmgr()->getWorld()->importBody("Body", QString(getenv("GRASPIT"))+ QString("/models/obstacles/zeroplane.xml"));
// graspItGUI->getIVmgr()->getWorld()->addBody(table);
return;
}
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();
}
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;
}
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;
}
void GraspPlanningTask::start()
{
//get the details of the planning task itself
if (!mDBMgr->GetPlanningTaskRecord(mRecord.taskId, &mPlanningTask)) {
DBGA("Failed to get planning record for task");
mStatus = FAILED;
return;
}
World *world = graspitCore->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() && world->getCurrentHand()->getDBName() == QString(mPlanningTask.handName.c_str())) {
DBGA("Grasp Planning Task: using currently loaded hand");
mHand = world->getCurrentHand();
} else {
QString handPath = mDBMgr->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 = FAILED;
return;
}
}
//check for virtual contacts
if (mHand->getNumVirtualContacts()==0) {
DBGA("Specified hand does not have virtual contacts defined");
mStatus = FAILED;
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 = FAILED;
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 LoopPlanner(mHand);
QObject::connect(mPlanner, SIGNAL(loopUpdate()), this, SLOT(plannerLoopUpdate()));
QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete()));
mPlanner->setEnergyType(ENERGY_CONTACT);
mPlanner->setContactType(CONTACT_PRESET);
mPlanner->setMaxSteps(65000);
mPlanner->setRepeat(true);
//max time set from database record
if (mPlanningTask.taskTime >= 0){
mPlanner->setMaxTime(mPlanningTask.taskTime);
} else {
mPlanner->setMaxTime(-1);
}
static_cast<SimAnnPlanner*>(mPlanner)->setModelState(&seed);
if (!mPlanner->resetPlanner()) {
DBGA("Grasp Planning Task: failed to reset planner");
mStatus = FAILED;
return ;
}
//load all already known grasps so that we avoid them in current searches
mDBMgr->SetGraspAllocator(new GraspitDBGraspAllocator(mHand));
std::vector<db_planner::Grasp*> graspList;
if(!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)){
//for now, we don't know if this means "no grasps found" or "error" so we assume the first
DBGA("No grasps found in database for model " << mPlanningTask.model->ModelName());
}
//and pass them on to the planner
for (size_t i=0; i<graspList.size(); i++) {
GraspPlanningState *state = new GraspPlanningState(static_cast<GraspitDBGrasp*>(graspList[i])
->getFinalGraspPlanningState() );
state->setObject(mObject);
state->setPositionType(SPACE_AXIS_ANGLE, true);
//careful here - is it the same posture in both spaces?
state->setPostureType(POSE_EIGEN, true);
static_cast<LoopPlanner*>(mPlanner)->addToAvoidList(state);
}
while (!graspList.empty()) {
delete graspList.back();
graspList.pop_back();
}
mLastSolution = mPlanner->getListSize();
DBGA("Planner starting off with " << mLastSolution << " solutions");
mPlanner->startPlanner();
mStatus = RUNNING;
}
