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