bool
EGPlanner::addSolution(GraspPlanningState *s)
{
bool addResult = addToListOfUniqueSolutions(s,&mBestList,0.02);
mCurrentStep +=1;
return addResult;
}
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");
}
/*! 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
SimAnnPlanner::mainLoop()
{
GraspPlanningState *input = NULL;
if ( processInput() ) {
input = mTargetState;
}
//call sim ann
SimAnn::Result result = mSimAnn->iterate(mCurrentState, mEnergyCalculator, input);
if ( result == SimAnn::FAIL) {
DBGP("Sim ann failed");
return;
}
DBGP("Sim Ann success");
//put result in list if there's room or it's better than the worst solution so far
double worstEnergy;
if ((int)mBestList.size() < BEST_LIST_SIZE) worstEnergy = 1.0e5;
else worstEnergy = mBestList.back()->getEnergy();
if (result == SimAnn::JUMP && mCurrentState->getEnergy() < worstEnergy) {
GraspPlanningState *insertState = new GraspPlanningState(mCurrentState);
//but check if a similar solution is already in there
if (!addToListOfUniqueSolutions(insertState,&mBestList,0.2)) {
delete insertState;
} else {
mBestList.sort(GraspPlanningState::compareStates);
while ((int)mBestList.size() > BEST_LIST_SIZE) {
delete(mBestList.back());
mBestList.pop_back();
}
}
}
render();
mCurrentStep = mSimAnn->getCurrentStep();
if (mCurrentStep % 100 == 0 && !mMultiThread) Q_EMIT update();
if (mMaxSteps == 200) {DBGP("Child at " << mCurrentStep << " steps");}
}
bool
EGPlanner::addSolution(GraspPlanningState *s)
{
return addToListOfUniqueSolutions(s,&mBestList,0.2);
}