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 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"); } }
/*! 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; }