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; } }
/*! Responds to the updates of the planner by attempting to save the most recently found grasps. */ void GraspPlanningTask::plannerLoopUpdate() { if (mStatus != RUNNING) return; //save all new solutions to database for(int i=mLastSolution; i<mPlanner->getListSize(); i++) { //copy the solution so we can change it GraspPlanningState *sol = new GraspPlanningState(mPlanner->getGrasp(i)); //convert it's tranform to the Quaternion__Translation format //make sure you pass it sticky=true, otherwise information is lost in the conversion sol->setPositionType(SPACE_COMPLETE,true); //we will want to save exact DOF positions, not eigengrasp values //again, make sure sticky=true sol->setPostureType(POSE_DOF,true); //we are ready to save it if (!saveGrasp(sol)) { DBGA("GraspPlanningState: failed to save solution to dbase"); mStatus = ERROR; break; } } //if something has gone wrong, stop the plan and return. if (mStatus == ERROR) { mPlanner->stopPlanner(); mStatus = ERROR; } else { DBGA(mPlanner->getListSize() - mLastSolution << " solutions saved to database"); } mLastSolution = mPlanner->getListSize(); }
void GraspPlanningTask::plannerLoopUpdate() { if (mStatus != RUNNING) return; //save all new solutions to database for(int i=mLastSolution; i<mPlanner->getListSize(); i++) { //copy the solution so we can change it GraspPlanningState *sol = new GraspPlanningState(mPlanner->getGrasp(i)); //convert it's tranform to the Quaternion__Translation format //make sure you pass it sticky=true, otherwise information is lost in the conversion sol->setPositionType(SPACE_COMPLETE,true); //we will want to save exact DOF positions, not eigengrasp values //again, make sure sticky=true sol->setPostureType(POSE_DOF,true); //we are ready to save it if (!saveGrasp(sol)) { DBGA("Grasp Planning Task: failed to save solution to dbase"); mStatus = FAILED; break; } } if (mStatus == FAILED) { // this is a bit of a hack, but ensures that the planner will stop // as soon as it attempts to take another step. If we specifically start // the planner from in here, it causes problem, as this is called from inside // the planner callback mPlanner->setMaxSteps(0); } else { DBGA(mPlanner->getListSize() - mLastSolution << " solutions saved to database"); } mLastSolution = mPlanner->getListSize(); }
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; } }
/*! Stops a child, and waits for the child thread to finish. After that, it retrieves the solutions found by the child, and places them in the list of solutions and also in the list of states to be avoided in the future. Solutions are also marked visually. Note that, for each solution recovered from a child, this function also reconstructs the final grasp (remember that solutions are usually pre-grasps) and saves BOTH in the list of solutions. */ void GuidedPlanner::stopChild(SimAnnPlanner *pl) { DBGA("Child has finished!"); //this sets the state to DONE which in turn waits for the thread to stop spinning pl->stopPlanner(); DBGP("Thread has stopped."); int j = pl->getListSize(); if (j) { //place a copy of the best state (which is the pre-grasp) in my best list GraspPlanningState *s = new GraspPlanningState( pl->getGrasp(0) ); s->printState(); //recall that child used its own cloned hand for planning s->changeHand(mHand, true); mBestList.push_back(s); //use this part to also save the final grasp from the child planner, //not just the pre-grasp pl->showGrasp(0); GraspPlanningState *finalGrasp = new GraspPlanningState(pl->getGrasp(0)); finalGrasp->setPositionType(SPACE_COMPLETE); //the final grasp is not in eigengrasp space, so we must save it in DOF space finalGrasp->setPostureType(POSE_DOF); finalGrasp->saveCurrentHandState(); //change the hand finalGrasp->changeHand(mHand, true); //and save it mBestList.push_back(finalGrasp); //place a copy of the pre-grasp in the avoid list and mark it red GraspPlanningState *s2 = new GraspPlanningState(s); mAvoidList.push_back(s2); mHand->getWorld()->getIVRoot()->addChild( s2->getIVRoot() ); if (s2->getEnergy() < 10.0) { s2->setIVMarkerColor(1,0,0); } else { s2->setIVMarkerColor(0.1,0.1,0.1); } DBGA("Enrgy from child: " << s2->getEnergy()); //the avoid list gets emptied at cleanup } else { DBGA("Child has no solutions"); } }
bool GuidedGraspPlanningTask::saveGrasp(const GraspPlanningState *pre_gps, const GraspPlanningState *final_gps) { GraspitDBModel* dbModel= mObject->getDBModel(); assert(dbModel); db_planner::Grasp* grasp = new db_planner::Grasp; std::vector<double> contacts = grasp->GetContacts(); grasp->SetSourceModel( *(static_cast<db_planner::Model*>(dbModel)) ); grasp->SetHandName(GraspitDBGrasp::getHandDBName(mHand).toStdString()); grasp->SetEpsilonQuality(final_gps->getEpsilonQuality()); grasp->SetVolumeQuality(final_gps->getVolume()); grasp->SetEnergy(final_gps->getEnergy()); grasp->SetClearance(0.0); grasp->SetClusterRep(false); grasp->SetCompliantCopy(false); grasp->SetSource("EIGENGRASPS"); std::vector<double> tempArray; //the final grasp GraspPlanningState *sol = new GraspPlanningState(final_gps); //make sure you pass it sticky=true sol->setPositionType(SPACE_COMPLETE,true); sol->setPostureType(POSE_DOF,true); tempArray.clear(); for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){ tempArray.push_back(sol->readPosture()->readVariable(i)); } grasp->SetFinalgraspJoints(tempArray); tempArray.clear(); for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){ tempArray.push_back(sol->readPosition()->readVariable(i)); } grasp->SetFinalgraspPosition(tempArray); delete sol; //the pre-grasp sol = new GraspPlanningState(pre_gps); sol->setPositionType(SPACE_COMPLETE,true); sol->setPostureType(POSE_DOF,true); tempArray.clear(); for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){ tempArray.push_back(sol->readPosture()->readVariable(i)); } grasp->SetPregraspJoints(tempArray); tempArray.clear(); for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){ tempArray.push_back(sol->readPosition()->readVariable(i)); } grasp->SetPregraspPosition(tempArray); delete sol; //contacts //for some reason, the grasp's contact vector gets initialized to a mess! tempArray.clear(); grasp->SetContacts(tempArray); std::vector<db_planner::Grasp*> graspList; graspList.push_back(grasp); bool result = mDBMgr->SaveGrasps(graspList); delete grasp; return result; }
/*! 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; }
void DBaseBatchPlanner::processSolution(const GraspPlanningState *s) { //we will write this solution to the result file //if it's a poor solution don't even bother if (s->getEnergy() > energyConstraint){ DBGAF(mLogStream,"Solution with energy to be thrown: " << s->getEnergy()); return; } //we need a SearchEnergy calculator in order to do the autoGrasp in the exact same way that the planner does it static SearchEnergy *se = NULL; //don't create it each time if (!se) { //this is the same type used by the loop planner switch(mType) { case DEXTEROUS: se = SearchEnergy::getSearchEnergy(ENERGY_STRICT_AUTOGRASP); break; case GRIPPER: se = SearchEnergy::getSearchEnergy(ENERGY_CONTACT); se->setContactType(CONTACT_PRESET); break; } } DBGAF(mLogStream,"Solution with energy: " << s->getEnergy()); //first, copy it to a new one so it's not const and we can modify it GraspPlanningState *sol = new GraspPlanningState(s); //convert it's tranform to the Quaternion__Translation format //make sure you pass it sticky=true, otherwise information is lost in the conversion sol->setPositionType(SPACE_COMPLETE,true); //we will want to save exact DOF positions, not eigengrasp values //again, make sure sticky=true sol->setPostureType(POSE_DOF,true); //we can write it to a file fprintf(mResultFile,"pre-grasp\n"); sol->writeToFile(mResultFile); //at this point, we are saving each DOF individually, but it should be an eigengrasp posture if (mType == DEXTEROUS) { //now close the fingers and perform the autograsp bool legal; double energy; //make sure to pass it noChange = false and it will leave the hand in the posture AFTER the autograsp //otherwise, analyzeState will revert the state to what it was on its entry se->analyzeState(legal,energy,sol,false); if (!legal) { DBGAF(mLogStream,"buru Illegal solution! This should not be!"); } //store the hand posture resulting from the autograsp //careful: this only works if sol is of the type SPACE_COMPLETE (as set above) sol->saveCurrentHandState(); //write this one to the file as well fprintf(mResultFile,"grasp\n"); sol->writeToFile(mResultFile); //this posture is probably not in eigengrasp space //write the contacts to a file fprintf(mResultFile,"contacts\n"); writeContactsToFile(sol->getHand(), sol->getObject()); } numOfGrasps++; printf("\nup to now, %d out of %d grasps have been found\n",numOfGrasps, numOfGraspsGoal); //add some codes here to limit the grasps if(numOfGrasps == numOfGraspsGoal) { mMaxTime = 10;// this is a trick, maybe not good printf("begin to leave..."); DBGAF(mLogStream, numOfGraspsGoal); mPlanner->setMaxTime(mMaxTime); } //we are done delete sol; }
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; }
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; }