/*! Does the usual main loop of a SimAnn planner, but checks if the current state is good enough to be placed in the list of seeds to be used for children. The list of seeds is also pruned to remove similar state, and only keep a list of "unique" seeds. */ void GuidedPlanner::mainLoop() { // call main simann iteration SimAnn::Result r = mSimAnn->iterate(mCurrentState, mEnergyCalculator); if (r==SimAnn::FAIL) return; //put result in list double bestEnergy; if ((int)mChildSeeds.size() < mChildSeedSize) { //only queue good states to begin with bestEnergy = mMinChildEnergy; } else { bestEnergy = mChildSeeds.back()->getEnergy(); } if (r==SimAnn::JUMP && mCurrentState->getEnergy() < bestEnergy) { GraspPlanningState *insertState = new GraspPlanningState(mCurrentState); DBGP("New solution. Is it a candidate?"); if (!addToListOfUniqueSolutions(insertState,&mChildSeeds,mDistanceThreshold)) { DBGP("No."); delete insertState; } else { DBGP("Yes"); //place a visual marker in the world mHand->getWorld()->getIVRoot()->addChild( insertState->getIVRoot() ); mChildSeeds.sort(GraspPlanningState::compareStates); DBGP("Queued..."); while ((int)mChildSeeds.size() > mChildSeedSize) { delete(mChildSeeds.back()); mChildSeeds.pop_back(); } DBGP("Done."); } } mCurrentStep = mSimAnn->getCurrentStep(); render(); if (mCurrentStep % 100 == 0) { emit update(); checkChildren(); } }
void OnLinePlanner::mainLoop() { static clock_t lastCheck = clock(); clock_t time = clock(); double secs = (float)(time - lastCheck) / CLOCKS_PER_SEC; if (secs < 0.2) { //perform grasp planning all the time graspLoop(); return; } lastCheck = time; //every 0.2 seconds, perform the management part: //set as a reference transform for the search the transform of the reference hand (presumably controlled by //the user via a flock of birds) mCurrentState->setRefTran( mRefHand->getTran(), false ); //this is to ensure this (potentially) illegal state does not make it into the best list mCurrentState->setLegal(false); //re-set the legal search range along the approach direction, so we don't search pointlessly inside the object if ( mCurrentState->getVariable("dist")) { Body *obj = mCurrentState->getObject(); double maxDist = 200; mObjectDistance = mRefHand->getApproachDistance(obj,maxDist); if (mObjectDistance > maxDist) mObjectDistance = maxDist; mCurrentState->getPosition()->getVariable("dist")->setRange(-30,mObjectDistance); //make sure the current value is within range; otherwise simm ann can hang... mCurrentState->getPosition()->getVariable("dist")->setValue(mObjectDistance/2); mCurrentState->getPosition()->getVariable("dist")->setJump(0.33); } //is the planning part has produced new candidates, send them to the grasp tester std::list<GraspPlanningState*>::iterator it = mCandidateList.begin(); while (it!=mCandidateList.end()) { //while there is space if ( mGraspTester->postCandidate(*it) ) { DBGP("Candidate posted"); it = mCandidateList.erase(it); } else { DBGP("Tester thread buffer is full"); break; } } //retrieve solutions from the tester GraspPlanningState *s; while ( (s = mGraspTester->popSolution()) != NULL ) { //hack - this is not ideal, but so far I don't have a better solution of how to keep track //of what hand is being used at what time s->changeHand( mRefHand, true );//CHANGED! to mSolutionClone from mRefHand mBestList.push_back(s); //graspItGUI->getIVmgr()->emitAnalyzeGrasp(s); if (mMarkSolutions) { mHand->getWorld()->getIVRoot()->addChild( s->getIVRoot() ); } } updateSolutionList(); //now shape the real hand. //s = mInterface->updateHand(&mBestList);//this used to update the current hand to solutions that are found, no longer does this // if (mBestList.size() != 0) showGrasp(0);//CHANGED! this replaces line above //CHANGED! stuff below was taken out // if (s) { // if (mSolutionClone) s->execute(mSolutionClone); // if (mMarkSolutions) s->setIVMarkerColor(1,1,0); // } DBGP("On-line main loop done"); }