C++ (Cpp) addToListOfUniqueSolutions Examples

Example #1

File: egPlanner.cpp Project: CURG/graspit_gdl

bool 
EGPlanner::addSolution(GraspPlanningState *s)
{   
  bool addResult = addToListOfUniqueSolutions(s,&mBestList,0.02);
  mCurrentStep +=1;
  return addResult;
}

Example #2

File: onLinePlanner.cpp Project: jon-weisz/graspit_bci

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

Example #3

File: guidedPlanner.cpp Project: BerkeleyAutomation/google_goggles_project

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

Example #4

File: simAnnPlanner.cpp Project: GraspControl/graspit

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

Example #5

bool 
EGPlanner::addSolution(GraspPlanningState *s)
{ 
  return addToListOfUniqueSolutions(s,&mBestList,0.2);
}