/*! The caller passes it a HandObjectState and a calculator that can be used
to compute the quality (or in annealing terms "energy") of a HandObjectState.
This function computes the next state in the annealing schedule.
See SimAnn::Result declaration for possible return values.
*/
SimAnn::Result
SimAnn::iterate(GraspPlanningState *currentState, SearchEnergy *energyCalculator, GraspPlanningState *targetState)
{
//using different cooling constants for probs and neighbors
double T = cooling(mT0, mParams.YC, mCurrentStep, mParams.YDIMS);
//attempt to compute a neighbor of the current state
GraspPlanningState *newState;
double energy; bool legal = false;
int attempts = 0; int maxAttempts = 10;
DBGP("Ngbr gen loop");
while (!legal && attempts <= maxAttempts) {
newState = stateNeighbor(currentState, T * mParams.NBR_ADJ, targetState);
DBGP("Analyze state...");
energyCalculator->analyzeState(legal, energy, newState);
DBGP("Analysis done.");
if (!legal) { delete newState; }
attempts++;
}
if (!legal) {
DBGP("Failed to compute a legal neighbor");
//we have failed to compute a legal neighbor.
//weather the SimAnn should advance a step and cool down the temperature even when it fails to compute a
//legal neighbor is debatable. Might be more interactive (especially for the online planner) if it does.
//mCurrentStep += 1;
return FAIL;
}
//we have a neighbor. Now we decide if we jump to it.
DBGP("Legal neighbor computed; energy: " << energy)
newState->setEnergy(energy);
newState->setLegal(true);
newState->setItNumber(mCurrentStep);
//using different cooling constants for probs and neighbors
T = cooling(mT0, mParams.HC, mCurrentStep, mParams.HDIMS);
double P = prob(mParams.ERR_ADJ * currentState->getEnergy(), mParams.ERR_ADJ * newState->getEnergy(), T);
double U = ((double)rand()) / RAND_MAX;
Result r = KEEP;
if (P > U) {
DBGP("Jump performed");
currentState->copyFrom(newState);
r = JUMP;
} else {
DBGP("Jump rejected");
}
mCurrentStep += 1; mTotalSteps += 1;
DBGP("Main iteration done.")
delete newState;
if (mWriteResults && mCurrentStep % 2 == 0) {
assert(mFile);
fprintf(mFile, "%ld %d %f %f %f %f\n", mCurrentStep, mTotalSteps, T, currentState->getEnergy(),
currentState->readPosition()->readVariable("Tx"),
targetState->readPosition()->readVariable("Tx"));
//currentState->writeToFile(mFile);
}
return r;
}
