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