bool CompliantGraspCopyTask::checkStoreGrasp(const db_planner::Grasp *original) { //sanity check if the world is in collision if (!mHand->getWorld()->noCollision()) { DBGA(" World is in collision"); return true; } //create the new grasp as a copy of the old one //this should copy score and everything const GraspitDBGrasp *graspit_original = static_cast<const GraspitDBGrasp *>(original); std::auto_ptr<GraspitDBGrasp> newGrasp(new GraspitDBGrasp(*graspit_original)); //new grasp is a compliant copy of the old one newGrasp->SetCompliantCopy(true); newGrasp->SetCompliantOriginalId(original->GraspId()); //compliant copy grasps are never cluster reps newGrasp->SetClusterRep(false); //set the grasp posture GraspPlanningState *graspState = new GraspPlanningState(mHand); graspState->setPostureType(POSE_DOF, false); graspState->setPositionType(SPACE_COMPLETE, false); graspState->setRefTran(mObject->getTran(), false); graspState->saveCurrentHandState(); newGrasp->setFinalGraspPlanningState(graspState); //prepare the pre-grasp GraspPlanningState *preGraspState = new GraspPlanningState(mHand); preGraspState->setPostureType(POSE_DOF, false); preGraspState->setPositionType(SPACE_COMPLETE, false); preGraspState->setRefTran(mObject->getTran(), false); //compute the pre-grasp posture; careful to leave the hand in the right place mHand->saveState(); bool pre_grasp_result = computePreGrasp(newGrasp.get()); preGraspState->saveCurrentHandState(); mHand->restoreState(); //check the pre-grasp if (!pre_grasp_result) { DBGA(" Pre-grasp creation fails"); return true; } //sanity check if (!mHand->getWorld()->noCollision()) { DBGA(" World is in collision AFTER PREGRASP COMPUTATION"); return true; } //set the pre-grasp newGrasp->setPreGraspPlanningState(preGraspState); //compute distance to object for pre grasp double clearance = mHand->getWorld()->getDist(mHand, mObject); newGrasp->SetClearance(clearance); //store the new grasp in the database if (!mDBMgr->SaveGrasp(newGrasp.get())) { DBGA(" Error writing new grasp to database"); return false; } return true; }
void CompliantPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z, double sz1, double sz2, vec3 tln, double res, std::list<GraspPlanningState*> *sampling) { mat3 R(x, y, z); int rotSamples=2; double m1 = (2.0*sz1 - floor(2.0*sz1 / res) * res)/2.0; while (m1 < 2*sz1){ double m2 = (2.0*sz2 - floor(2.0*sz2 / res) * res)/2.0; while (m2 < 2*sz2) { vec3 myTln(tln); myTln = myTln + (m1 - sz1)* y; myTln = myTln + (m2 - sz2)* z; transf tr(R, myTln); for(int rot=0; rot < rotSamples; rot++) { double angle = M_PI * ((double)rot) / rotSamples; transf rotTran(Quaternion(angle, vec3(1,0,0)), vec3(0,0,0)); tr = rotTran * tr; GraspPlanningState* seed = new GraspPlanningState(mHand); seed->setObject(mObject); seed->setRefTran(mObject->getTran(), false); seed->setPostureType(POSE_DOF, false); seed->setPositionType(SPACE_COMPLETE, false); seed->reset(); seed->getPosition()->setTran(tr); sampling->push_back(seed); } m2+=res; } m1 += res; } }
OnLinePlanner * createDefaultPlanner(){ World * w = getWorld(); if(!w->getCurrentHand()) { DBGA("plannerTools::createDefaultPlanner::No current Hand!"); return NULL; } GraspPlanningState *mHandObjectState = new GraspPlanningState(w->getCurrentHand()); mHandObjectState->setPositionType(SPACE_AXIS_ANGLE); mHandObjectState->setObject(w->getGB(0)); mHandObjectState->setRefTran(w->getGB(0)->getTran()); mHandObjectState->reset(); OnLinePlanner * op = new OnLinePlanner(w->getCurrentHand()); op->setContactType(CONTACT_PRESET); op->setEnergyType(ENERGY_CONTACT_QUALITY); op->setMaxSteps(2000); op->setModelState(mHandObjectState); w->setCurrentPlanner(op); op->showSolutionClone(true); op->resetPlanner(); return op; }
void GraspTester::mainLoop() { GraspPlanningState *s = popCandidate(); if (!s) { DBGP("Empty buffer for tester"); msleep(100); return; } s->changeHand(mHand, true); testGrasp(s); DBGP("TESTER: candidate has energy " << s->getEnergy()); mHand->breakContacts(); if (s->isLegal() && s->getEnergy() < -1.2) { //save the final grasping position that has resulted from autograsp s->setPositionType(SPACE_COMPLETE); s->setPostureType(POSE_DOF); //save the current transform in absolute terms s->setRefTran(transf::IDENTITY); s->saveCurrentHandState(); postSolution(s); DBGP("Tester posting a solution at iteration " << s->getItNumber()); } else { DBGP("Tester removing candidate"); delete s; } }
void importGraspsFromDBMgr( OnLinePlanner * mPlanner, db_planner::DatabaseManager * mDbMgr) { Hand*mHand = mPlanner->getHand(); // Get corresponding model from database std::vector<db_planner::Model*> modelList; QString *objectFilename = new QString('/' + mHand->getGrasp()->getObject()->getFilename().split('/').back()); mDbMgr->ModelList(&modelList,db_planner::FilterList::USE_FILE_NAME, *objectFilename); if(modelList.empty()) { DBGA("No Models Found \n"); return; } // Using the found model, retrieve the grasps std::vector<db_planner::Grasp*> grasps; mDbMgr->GetGrasps(*modelList.back(), GraspitDBGrasp::getHandDBName(mHand).toStdString(), &grasps); mHand->saveState(); bool collisionState = getWorld()->collisionsAreOff(mPlanner->getHand()); getWorld()->toggleCollisions(true, mPlanner->getHand()); bool targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); // Load the grasps into the grasp planner list. unsigned int numGrasps = std::min<unsigned int>(grasps.size(), 10); for (unsigned int gNum = 0; gNum < numGrasps; ++gNum) { GraspPlanningState *s = new GraspPlanningState(static_cast<GraspitDBGrasp *> (grasps[gNum])->getFinalGraspPlanningState()); s->setObject(mHand->getGrasp()->getObject()); s->setRefTran(mHand->getGrasp()->getObject()->getTran(), false); targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); float testResult = -2*bci_experiment::planner_tools::testGraspCollisions(mHand, s); while(targetsOff) { getWorld()->toggleCollisions(true, mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); } s->addAttribute("graspId", gNum); s->addAttribute("testResult", testResult); s->addAttribute("testTime", 0); mPlanner->addSolution(s); } //reorders the solutions we have found. mPlanner->updateSolutionList(); targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); //needed to return hand to aligned with object, since it was used to testGraspCollisions getWorld()->toggleCollisions(!collisionState, mPlanner->getHand()); mHand->restoreState(); targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); }
void OnLinePlanner::graspLoop() { //DBGP("Grasp loop started"); //prepare input GraspPlanningState *input = NULL; if ( processInput() ) { input = mTargetState; } //call simulated annealing SimAnn::Result r = mSimAnn->iterate(mCurrentState,mEnergyCalculator,input); mCurrentStep = mSimAnn->getCurrentStep(); if ( r == SimAnn::JUMP ) { assert(mCurrentState->isLegal()); //we have a new state from the SimAnn if (mCurrentState->getEnergy() < 0 || mCurrentState->getEnergy() < mCurrentBest->getEnergy()) { DBGP("New candidate"); GraspPlanningState *insertState = new GraspPlanningState(mCurrentState); //make solution independent of reference hand position insertState->setPositionType(SPACE_COMPLETE,true); insertState->setRefTran( mCurrentState->getObject()->getTran(), true); insertState->setItNumber( mCurrentStep ); if (insertState->getEnergy() < mCurrentBest->getEnergy()) { mCurrentBest->copyFrom( insertState ); } if (!addToListOfUniqueSolutions(insertState, &mCandidateList,0.4)) { DBGP("Similar to old candidate"); delete insertState; } else { //graspItGUI->getIVmgr()->emitAnalyzeGrasp(mCandidateList.back()); mCandidateList.sort(GraspPlanningState::compareStates);//CHANGED! was compareStates while (mCandidateList.size() > CANDIDATE_BUFFER_SIZE) { delete mCandidateList.back(); mCandidateList.pop_back(); } } DBGP("Added candidate"); } } if (mCurrentStep % 100 == 0) emit update(); render(); //DBGP("Grasp loop done"); }
/*! Every grasp in graspList will be tested and stored in testedGraspList even if their test scores, epsilon quality and volume quality, are not positive, which means they are not stable grasps. */ bool GraspitDBPlanner::testGrasps(TestType t, std::vector<db_planner::Grasp *> graspList, std::vector<db_planner::Grasp *> *testedGraspList) { mTestedGrasps.clear(); if (testedGraspList) { testedGraspList->clear(); } // test each of the grasps in graspList for (int i = 0; i < ((int)graspList.size()); i ++) { GraspitDBGrasp *tempGrasp = new GraspitDBGrasp(*static_cast<GraspitDBGrasp *>(graspList[i])); // load this grasp into the world static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->setTran(transf::IDENTITY); GraspPlanningState *tempState = new GraspPlanningState(tempGrasp->getPreGraspPlanningState()); tempState->setRefTran(transf::IDENTITY); tempGrasp->setPreGraspPlanningState(tempState); float elmts[16]; if (mAligner->Align(tempGrasp->SourceModel(), *mObject, elmts)) { tempGrasp->Transform(elmts); } mHand->getGrasp()->setObject(static_cast<GraspitDBModel *>(mObject)->getGraspableBody()); // apply it to the hand tempGrasp->getPreGraspPlanningState()->execute(); bool testResult; DynamicCode dynCode; // test this grasp if (t == STATIC) { testResult = testGraspStatic(); dynCode = NO_DYNAMICS; } else if (t == DYNAMIC) { testResult = testGraspDynamic(&dynCode); //DBGA("Result: " << testResult << "; dynamics code: " << dynCode); } float eq = -1.0, vq; // decide whether to record if (testResult) { computeQuality(eq, vq); if (!static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getNumContacts()) { dynCode = DYNAMIC_OBJECT_EJECTED; } else if (eq <= 0.0) { dynCode = DYNAMIC_NO_FC; } } else { return false; } if (eq < 0) { continue; } //we now save all grasps in mTestedGrasps GraspPlanningState *newGrasp = new GraspPlanningState(mHand); newGrasp->setPositionType(SPACE_COMPLETE, false); newGrasp->setPostureType(POSE_DOF, false); //it is possible the object moved after dynamics, so we need to re-set the reference posture newGrasp->setRefTran(static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getTran()); newGrasp->saveCurrentHandState(); newGrasp->setEpsilonQuality(eq); newGrasp->setVolume(vq); newGrasp->setIndex(i); //this is a hack; just gives us a field to save the dynamics code newGrasp->setDistance((double)dynCode); mTestedGrasps.push_back(newGrasp); //std::cout << "grasp tested, quality: " << eq << std::endl; if (!testedGraspList) { continue; } // record this to synthesize the output db_planner::Grasp *recordGrasp = static_cast<db_planner::Grasp *>(new GraspitDBGrasp(*tempGrasp)); tempState = new GraspPlanningState(static_cast<GraspitDBGrasp *>(recordGrasp)->getPreGraspPlanningState()); tempState->copyFrom(newGrasp); static_cast<GraspitDBGrasp *>(recordGrasp)->setPreGraspPlanningState(tempState); testedGraspList->push_back(recordGrasp); delete tempGrasp; } if (mTestedGrasps.size() != 0) { return true; } return false; }
bool PreGraspCheckTask::checkSetGrasp(db_planner::Grasp *grasp) { if (!computePreGrasp(grasp)) { DBGA("Pre-grasp creation fails"); //delete from database if (!mDBMgr->DeleteGrasp(grasp)) { DBGA("Failed to delete grasp with id " << grasp->GraspId() << " from database"); return false; } return true; } //sanity check if (!mHand->getWorld()->noCollision()) { DBGA("Collision detected for pre-grasp!"); if (!mDBMgr->DeleteGrasp(grasp)) { DBGA("Failed to delete grasp with id " << grasp->GraspId() << " from database"); return false; } return true; } //compute distance to object double clearance = mHand->getWorld()->getDist(mHand, mObject); grasp->SetClearance(clearance); //create pre-grasp GraspPlanningState *newPreGrasp = new GraspPlanningState(mHand); newPreGrasp->setPostureType(POSE_DOF, false); newPreGrasp->setPositionType(SPACE_COMPLETE, false); newPreGrasp->setRefTran(mObject->getTran(), false); newPreGrasp->saveCurrentHandState(); //set pre-grasp static_cast<GraspitDBGrasp *>(grasp)->setPreGraspPlanningState(newPreGrasp); //save pre-grasp along with clearance in database //for now, we update by deleting and re-inserting if (!mDBMgr->DeleteGrasp(grasp)) { DBGA("Failed to delete grasp with id " << grasp->GraspId() << " from database"); return false; } /* //move the grasp back by 42mm transf t(Quaternion::IDENTITY, vec3(-42, 0, 0)); GraspitDBGrasp* dbg = static_cast<GraspitDBGrasp*>(grasp); GraspPlanningState *movedPreGrasp = new GraspPlanningState(dbg->getPreGraspPlanningState()); movedPreGrasp->getPosition()->setTran( t * movedPreGrasp->getPosition()->getCoreTran() ); dbg->setPreGraspPlanningState(movedPreGrasp); GraspPlanningState *movedFinalGrasp = new GraspPlanningState(dbg->getFinalGraspPlanningState()); movedFinalGrasp->getPosition()->setTran( t * movedFinalGrasp->getPosition()->getCoreTran() ); dbg->setFinalGraspPlanningState(movedFinalGrasp); */ //and save to database if (!mDBMgr->SaveGrasp(grasp)) { DBGA("Failed to save new grasp to database"); return false; } DBGA("Pre-grasp inserted"); return true; }