void ARAPlanner::Reevaluatefvals(ARASearchStateSpace_t* pSearchStateSpace)
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->heap;
//recompute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i) {
ARAState* state = (ARAState*)pheap->heap[i].heapstate;
pheap->heap[i].key.key[0] = state->g + (int)(pSearchStateSpace->eps * state->h);
//pheap->heap[i].key.key[1] = state->h;
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
//note this does NOT re-compute heuristics, only re-orders OPEN list based on current eps and h-vals
void RSTARPlanner::Reevaluatefvals()
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->OPEN;
//re-compute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i)
{
RSTARState* state = (RSTARState*)pheap->heap[i].heapstate;
pheap->heap[i].key = ComputeKey(state);
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
void ADPlanner::Reevaluatefvals(ADSearchStateSpace_t* pSearchStateSpace)
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->heap;
#if DEBUG
SBPL_FPRINTF(fDeb, "re-computing heap priorities\n");
#endif
//recompute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i) {
ADState* state = (ADState*)pheap->heap[i].heapstate;
pheap->heap[i].key = ComputeKey(state);
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
void TRAPlanner::Reevaluatefvals(TRASearchStateSpace_t* pSearchStateSpace)
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->heap;
#if DEBUG
SBPL_FPRINTF(fDeb, "re-computing heap priorities\n");
#endif
//recompute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i)
{
TRAState* state = (TRAState*)pheap->heap[i].heapstate;
key.key[0] = state->g + (int)(pSearchStateSpace->eps*state->h);
pheap->heap[i].key = key;
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
void anaPlanner::Reevaluatefvals(anaSearchStateSpace_t* pSearchStateSpace)
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->heap;
//recompute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i) {
//anaState* state = (anaState*)pheap->heap[i].heapstate;
// CHANGED - cast removed
pheap->heap[i].key.key[0] = (long)-get_e_value(pSearchStateSpace,
((anaState*)pheap->heap[i].heapstate)->MDPstate->StateID);
//pheap->heap[i].key.key[1] = state->h;
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
bool anaPlanner::Search(anaSearchStateSpace_t* pSearchStateSpace, vector<int>& pathIds, int & PathCost,
bool bFirstSolution, bool bOptimalSolution, double MaxNumofSecs)
{
CKey key;
TimeStarted = clock();
searchexpands = 0;
#if DEBUG
fprintf(fDeb, "new search call (call number=%d)\n", pSearchStateSpace->callnumber);
#endif
if (pSearchStateSpace->bReinitializeSearchStateSpace == true) {
//re-initialize state space
ReInitializeSearchStateSpace(pSearchStateSpace);
}
if (bOptimalSolution) {
pSearchStateSpace->eps = 1;
MaxNumofSecs = INFINITECOST;
}
else if (bFirstSolution) {
MaxNumofSecs = INFINITECOST;
}
//ensure heuristics are up-to-date
environment_->EnsureHeuristicsUpdated((bforwardsearch == true));
//the main loop of ana*
int prevexpands = 0;
clock_t loop_time;
// CHANGE MADE TO WHILE LOOP to account for open.empty() == FALSE
while (!pSearchStateSpace->heap->emptyheap() && pSearchStateSpace->eps_satisfied > ana_FINAL_EPS &&
(clock() - TimeStarted) < MaxNumofSecs * (double)CLOCKS_PER_SEC)
{
loop_time = clock();
//decrease eps for all subsequent iterations
/*if(fabs(pSearchStateSpace->eps_satisfied - pSearchStateSpace->eps) < ERR_EPS && !bFirstSolution)
{
pSearchStateSpace->eps = pSearchStateSpace->eps - ana_DECREASE_EPS;
if(pSearchStateSpace->eps < ana_FINAL_EPS)
pSearchStateSpace->eps = ana_FINAL_EPS;
//the priorities need to be updated
pSearchStateSpace->bReevaluatefvals = true;
//it will be a new search
pSearchStateSpace->bNewSearchIteration = true;
//build a new open list by merging it with incons one
BuildNewOPENList(pSearchStateSpace);
}*/
pSearchStateSpace->searchiteration++;
pSearchStateSpace->bNewSearchIteration = false;
//re-compute f-values if necessary and reorder the heap
//if(pSearchStateSpace->bReevaluatefvals)
// Reevaluatefvals(pSearchStateSpace);
//improve or compute path
int retVal = ImprovePath(pSearchStateSpace, MaxNumofSecs);
anaState* state;
CKey key;
CHeap* open = pSearchStateSpace->heap;
//printf("states expanded: %d\t states considered: %d\t time elapsed: %f\n",searchexpands - prevexpands, pSearchStateSpace->heap->currentsize, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
double epsprime = 1.0;
for (int j = 1; j <= open->currentsize;) {
state = (anaState*)open->heap[j].heapstate;
double temp_eps = (double)((pSearchStateSpace->G * 1.0) / (double)(state->g + state->h));
if (temp_eps > epsprime) {
epsprime = temp_eps;
}
double e_val = get_e_value(pSearchStateSpace, state->MDPstate->StateID);
if (e_val <= 1.0) {
open->deleteheap_unsafe(state);
}
else {
key.key[0] = (long)-e_val;
open->updateheap_unsafe(state, key);
++j;
}
pSearchStateSpace->eps_satisfied = epsprime;
}
open->makeheap();
//print the solution cost and eps bound
if (retVal == 1) {
//printf("suboptimality=%f expands=%d g(searchgoal)=%d loop_time=%.3f time_elapsed=%.3f memoryCounter=%d\n", pSearchStateSpace->eps_satisfied, searchexpands - prevexpands, ((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g,double(clock()-loop_time)/CLOCKS_PER_SEC, double(clock() - TimeStarted)/CLOCKS_PER_SEC, MaxMemoryCounter);
printf("suboptimality=%f g(searchgoal)=%d time_elapsed=%.3f memoryCounter=%d\n",
pSearchStateSpace->eps_satisfied,
((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g, double(clock()
- TimeStarted) / CLOCKS_PER_SEC, MaxMemoryCounter);
//printf("states expanded: %d\t states considered: %d\t time elapsed: %f\n",searchexpands - prevexpands, pSearchStateSpace->heap->currentsize, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
}
#if DEBUG
fprintf(fDeb, "eps=%f expands=%d g(searchgoal)=%d time=%.3f\n", pSearchStateSpace->eps_satisfied, searchexpands - prevexpands,
((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g,double(clock()-loop_time)/CLOCKS_PER_SEC);
PrintSearchState((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData, fDeb);
#endif
prevexpands = searchexpands;
//if just the first solution then we are done
if (bFirstSolution) break;
//no solution exists
if (((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g == INFINITECOST) break;
}
#if DEBUG
fflush(fDeb);
#endif
printf("Suboptimality = %.4f\n", pSearchStateSpace->eps_satisfied);
PathCost = ((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g;
MaxMemoryCounter += environment_->StateID2IndexMapping.size() * sizeof(int);
printf("MaxMemoryCounter = %d\n", MaxMemoryCounter);
int solcost = INFINITECOST;
bool ret = false;
if (PathCost == INFINITECOST) {
printf("could not find a solution\n");
ret = false;
}
else {
printf("solution is found\n");
pathIds = GetSearchPath(pSearchStateSpace, solcost);
ret = true;
}
printf("total expands this call = %d, planning time = %.3f secs, solution cost=%d\n", searchexpands, (clock()
- TimeStarted) / ((double)CLOCKS_PER_SEC), solcost);
//fprintf(fStat, "%d %d\n", searchexpands, solcost);
return ret;
}