ANAplanner.cpp
Go to the documentation of this file.
00001 /*
00002  * This code was used for generating experimental data for the purpose of understanding the performance of
00003  * the Anytime Nonparametric A* (ANA*) algorithm.  
00004  * The authors of this algorithm are Jur van den Berg, Rajat Shah, Arthur Huang and Ken Goldberg.
00005  * The code is available at http://goldberg.berkeley.edu/ana/
00006  * 
00007  */
00008 
00009 
00010 #include <iostream>
00011 using namespace std;
00012 
00013 #include "../../sbpl/headers.h"
00014 
00015 
00016 
00017 
00018 //-----------------------------------------------------------------------------------------------------
00019 
00020 anaPlanner::anaPlanner(DiscreteSpaceInformation* environment, bool bSearchForward)
00021 {
00022         bforwardsearch = bSearchForward;
00023 
00024     environment_ = environment;
00025     
00026         bsearchuntilfirstsolution = false;
00027     finitial_eps = ana_DEFAULT_INITIAL_EPS;
00028     searchexpands = 0;
00029     MaxMemoryCounter = 0;
00030     
00031     fDeb = fopen("debug.txt", "w");
00032     
00033     pSearchStateSpace_ = new anaSearchStateSpace_t;
00034     
00035     
00036     //create the ana planner
00037     if(CreateSearchStateSpace(pSearchStateSpace_) != 1)
00038         {
00039             printf("ERROR: failed to create statespace\n");
00040             return;
00041         }
00042     
00043     //set the start and goal states
00044     if(InitializeSearchStateSpace(pSearchStateSpace_) != 1)
00045         {
00046             printf("ERROR: failed to create statespace\n");
00047             return;
00048         }    
00049 }
00050 
00051 anaPlanner::~anaPlanner()
00052 {
00053   if(pSearchStateSpace_ != NULL){
00054     //delete the statespace
00055     DeleteSearchStateSpace(pSearchStateSpace_);
00056     delete pSearchStateSpace_;
00057   }
00058   fclose(fDeb);
00059 }
00060 
00061 
00062 
00063 
00064 
00065 void anaPlanner::Initialize_searchinfo(CMDPSTATE* state, anaSearchStateSpace_t* pSearchStateSpace)
00066 {
00067 
00068         anaState* searchstateinfo = (anaState*)state->PlannerSpecificData;
00069 
00070         searchstateinfo->MDPstate = state;
00071         InitializeSearchStateInfo(searchstateinfo, pSearchStateSpace); 
00072 }
00073 
00074 
00075 CMDPSTATE* anaPlanner::CreateState(int stateID, anaSearchStateSpace_t* pSearchStateSpace)
00076 {       
00077         CMDPSTATE* state = NULL;
00078 
00079 #if DEBUG
00080         if(environment_->StateID2IndexMapping[stateID][anaMDP_STATEID2IND] != -1)
00081         {
00082                 printf("ERROR in CreateState: state already created\n");
00083                 exit(1);
00084         }
00085 #endif
00086 
00087         //adds to the tail a state
00088         state = pSearchStateSpace->searchMDP.AddState(stateID);
00089 
00090         //remember the index of the state
00091         environment_->StateID2IndexMapping[stateID][anaMDP_STATEID2IND] = pSearchStateSpace->searchMDP.StateArray.size()-1;
00092 
00093 #if DEBUG
00094         if(state != pSearchStateSpace->searchMDP.StateArray[environment_->StateID2IndexMapping[stateID][anaMDP_STATEID2IND]])
00095         {
00096                 printf("ERROR in CreateState: invalid state index\n");
00097                 exit(1);
00098         }
00099 #endif
00100 
00101 
00102         //create search specific info
00103         state->PlannerSpecificData = (anaState*)malloc(sizeof(anaState));       
00104         Initialize_searchinfo(state, pSearchStateSpace);
00105         MaxMemoryCounter += sizeof(anaState);
00106 
00107         return state;
00108 
00109 }
00110 
00111 CMDPSTATE* anaPlanner::GetState(int stateID, anaSearchStateSpace_t* pSearchStateSpace)
00112 {       
00113 
00114         if(stateID >= (int)environment_->StateID2IndexMapping.size())
00115         {
00116                 SBPL_ERROR("ERROR in GetState: stateID %d is invalid\n", stateID);
00117                 throw new SBPL_Exception();
00118         }
00119 
00120         if(environment_->StateID2IndexMapping[stateID][anaMDP_STATEID2IND] == -1)
00121                 return CreateState(stateID, pSearchStateSpace);
00122         else
00123                 return pSearchStateSpace->searchMDP.StateArray[environment_->StateID2IndexMapping[stateID][anaMDP_STATEID2IND]];
00124 
00125 }
00126 
00127 
00128 
00129 //-----------------------------------------------------------------------------------------------------
00130 
00131 
00132 
00133 
00134 
00135 
00136 
00137 
00138 int anaPlanner::ComputeHeuristic(CMDPSTATE* MDPstate, anaSearchStateSpace_t* pSearchStateSpace)
00139 {
00140         //compute heuristic for search
00141 
00142         if(bforwardsearch)
00143         {
00144 
00145 #if MEM_CHECK == 1
00146                 //int WasEn = DisableMemCheck();
00147 #endif
00148 
00149                 //forward search: heur = distance from state to searchgoal which is Goal anaState
00150                 int retv =  environment_->GetGoalHeuristic(MDPstate->StateID);
00151 
00152 #if MEM_CHECK == 1
00153                 //if (WasEn)
00154                 //      EnableMemCheck();
00155 #endif
00156 
00157                 return retv;
00158 
00159         }
00160         else
00161         {
00162                 //backward search: heur = distance from searchgoal to state
00163                 return environment_->GetStartHeuristic(MDPstate->StateID);
00164         }
00165 }
00166 
00167 
00168 //initialization of a state
00169 void anaPlanner::InitializeSearchStateInfo(anaState* state, anaSearchStateSpace_t* pSearchStateSpace)
00170 {
00171         state->g = INFINITECOST;
00172         state->v = INFINITECOST;
00173         state->iterationclosed = 0;
00174         state->callnumberaccessed = pSearchStateSpace->callnumber;
00175         state->bestnextstate = NULL;
00176         state->costtobestnextstate = INFINITECOST;
00177         state->heapindex = 0;
00178         state->listelem[ana_INCONS_LIST_ID] = 0;
00179         state->numofexpands = 0;
00180 
00181         state->bestpredstate = NULL;
00182 
00183         //compute heuristics
00184 #if USE_HEUR
00185         if(pSearchStateSpace->searchgoalstate != NULL)
00186                 state->h = ComputeHeuristic(state->MDPstate, pSearchStateSpace); 
00187         else 
00188                 state->h = 0;
00189 #else
00190         state->h = 0;
00191 #endif
00192 
00193 
00194 }
00195 
00196 
00197 
00198 //re-initialization of a state
00199 void anaPlanner::ReInitializeSearchStateInfo(anaState* state, anaSearchStateSpace_t* pSearchStateSpace)
00200 {
00201         state->g = INFINITECOST;
00202         state->v = INFINITECOST;
00203         state->iterationclosed = 0;
00204         state->callnumberaccessed = pSearchStateSpace->callnumber;
00205         state->bestnextstate = NULL;
00206         state->costtobestnextstate = INFINITECOST;
00207         state->heapindex = 0;
00208         state->listelem[ana_INCONS_LIST_ID] = 0;
00209         state->numofexpands = 0;
00210 
00211         state->bestpredstate = NULL;
00212 
00213         //compute heuristics
00214 #if USE_HEUR
00215 
00216         if(pSearchStateSpace->searchgoalstate != NULL)
00217         {
00218                 state->h = ComputeHeuristic(state->MDPstate, pSearchStateSpace); 
00219         }
00220         else 
00221                 state->h = 0;
00222 
00223 #else
00224 
00225         state->h = 0;
00226 
00227 #endif
00228 
00229 
00230 }
00231 
00232 
00233 
00234 void anaPlanner::DeleteSearchStateData(anaState* state)
00235 {
00236         //no memory was allocated
00237         MaxMemoryCounter = 0;
00238         return;
00239 }
00240 
00241 
00242 
00243 
00244 double anaPlanner::get_e_value(anaSearchStateSpace_t* pSearchStateSpace, int stateID) {
00245 
00246         CMDPSTATE* MDPstate = GetState(stateID, pSearchStateSpace);
00247         anaState* searchstateinfo = (anaState*)MDPstate->PlannerSpecificData;
00248         
00249         //if(!(searchstateinfo->g > pSearchStateSpace->G)) {
00250         if(searchstateinfo->h == 0) {
00251                 if(searchstateinfo->g >= pSearchStateSpace->G) {
00252                         return 0.0;
00253                 } else {
00254                         return (double) INFINITECOST;
00255                 }
00256         } else {
00257                 return  ((double) pSearchStateSpace->G - 1.0*searchstateinfo->g) / (double) searchstateinfo->h;
00258 
00259                 //return  0.5*(((double) pSearchStateSpace->G - 1.0*searchstateinfo->g) / (double) searchstateinfo->h);
00260 
00261                 //return  1000 + (((double) pSearchStateSpace->G - 1.0*searchstateinfo->g) / (double) searchstateinfo->h);
00262 
00263         }
00264 } //else {
00265 
00266 
00267 
00268 
00269 //used for backward search
00270 void anaPlanner::UpdatePreds(anaState* state, anaSearchStateSpace_t* pSearchStateSpace) {
00271     vector<int> PredIDV;
00272     vector<int> CostV;
00273         CKey key;
00274         anaState *p;
00275 
00276     environment_->GetPreds(state->MDPstate->StateID, &PredIDV, &CostV);
00277 
00278         //iterate through predecessors of s
00279         for(int pind = 0; pind < (int)PredIDV.size(); pind++)
00280         {
00281                 CMDPSTATE* PredMDPState = GetState(PredIDV[pind], pSearchStateSpace);
00282                 p = (anaState*)(PredMDPState->PlannerSpecificData);
00283                 if(p->callnumberaccessed != pSearchStateSpace->callnumber)
00284                         ReInitializeSearchStateInfo(p, pSearchStateSpace);
00285 
00286                 //see if we can improve the value of p
00287                 
00288                 if((p->g > state->g + CostV[pind]) && (state->g + CostV[pind] + p->h < pSearchStateSpace->G)) {
00289                         p->g = state->g + CostV[pind];
00290                         p->bestnextstate = state->MDPstate;
00291                         p->costtobestnextstate = CostV[pind];
00292 
00293                         key.key[0] = (long)-get_e_value(pSearchStateSpace, p->MDPstate->StateID);
00294                         if(pSearchStateSpace->heap->inheap(p)) {
00295                                 pSearchStateSpace->heap->updateheap(p, key);
00296                         } else {
00297                                 pSearchStateSpace->heap->insertheap(p, key);
00298                         }
00299                 }
00300         }
00301 }
00302 
00303 
00304 
00305 //used for forward search
00306 void anaPlanner::UpdateSuccs(anaState* state, anaSearchStateSpace_t* pSearchStateSpace) {
00307     vector<int> SuccIDV;
00308     vector<int> CostV;
00309         CKey key;
00310         anaState *n;
00311 
00312     environment_->GetSuccs(state->MDPstate->StateID, &SuccIDV, &CostV);
00313 
00314         //iterate through predecessors of s
00315         for(int sind = 0; sind < (int)SuccIDV.size(); sind++) {
00316                 CMDPSTATE* SuccMDPState = GetState(SuccIDV[sind], pSearchStateSpace);
00317                 int cost = CostV[sind];
00318 
00319                 n = (anaState*)(SuccMDPState->PlannerSpecificData);
00320                 if(n->callnumberaccessed != pSearchStateSpace->callnumber)
00321                         ReInitializeSearchStateInfo(n, pSearchStateSpace);
00322 
00323                 //see if we can improve the value of n
00324                 //taking into account the cost of action
00325                 if((n->g > state->g + cost) && ((state->g + cost + n->h) < pSearchStateSpace->G)) {
00326                         n->g = state->g + cost;
00327                         n->bestpredstate = state->MDPstate; 
00328                         
00329                         
00330                         key.key[0] = (long)-get_e_value(pSearchStateSpace, n->MDPstate->StateID);
00331                         /*if(key.key[0] >= -1) {
00332                                 printf("inserting on Open with key =%d\n", key.key[0]);
00333                         }*/
00334                         if(pSearchStateSpace->heap->inheap(n)) {
00335                                 pSearchStateSpace->heap->updateheap(n, key);
00336                         } else {
00337                                 pSearchStateSpace->heap->insertheap(n, key);
00338                         }
00339                 }
00340         }
00341 }
00342 
00343 
00344 
00345 //TODO-debugmax - add obsthresh and other thresholds to other environments in 3dkin
00346 int anaPlanner::GetGVal(int StateID, anaSearchStateSpace_t* pSearchStateSpace)
00347 {
00348          CMDPSTATE* cmdp_state = GetState(StateID, pSearchStateSpace);
00349          anaState* state = (anaState*)cmdp_state->PlannerSpecificData;
00350          return state->g;
00351 }
00352 
00353 //returns 1 if the solution is found, 0 if the solution does not exist and 2 if it ran out of time
00354 int anaPlanner::ImprovePath(anaSearchStateSpace_t* pSearchStateSpace, double MaxNumofSecs) {
00355         int expands;
00356         anaState *state, *searchgoalstate;
00357         CKey key, minkey;
00358         //CKey goalkey;
00359 
00360         expands = 0;
00361 
00362 
00363         if(pSearchStateSpace->searchgoalstate == NULL)
00364         {
00365                 SBPL_ERROR("ERROR searching: no goal state is set\n");
00366                 throw new SBPL_Exception();
00367         }
00368 
00369         //goal state
00370         searchgoalstate = (anaState*)(pSearchStateSpace->searchgoalstate->PlannerSpecificData);
00371         if(searchgoalstate->callnumberaccessed != pSearchStateSpace->callnumber)
00372                 ReInitializeSearchStateInfo(searchgoalstate, pSearchStateSpace);
00373 
00374         //set goal key
00375         //goalkey.key[0] = -get_e_value(pSearchStateSpace, searchgoalstate->MDPstate->StateID);
00376         //goalkey.key[1] = searchgoalstate->h;
00377 
00378         //expand states until done
00379         minkey.key[0] = -(pSearchStateSpace->heap->getminkeyheap().key[0]);
00380         CKey oldkey = minkey;
00381         while(!pSearchStateSpace->heap->emptyheap() &&
00382                 (clock()-TimeStarted) < MaxNumofSecs*(double)CLOCKS_PER_SEC) //&& goalkey > minkey && minkey.key[0] <= INFINITECOST
00383     {
00384                 /*if(minkey.key[0] < 10) {
00385                         printf("Key: %.2f\t", minkey.key[0]);
00386                 }*/
00387                 //printf("%.2f\t", minkey.key[0]);
00388         
00389 
00390                 //get the state         
00391                 state = (anaState*)pSearchStateSpace->heap->deleteminheap();
00392         
00393                 
00394                 if(state->MDPstate->StateID == searchgoalstate->MDPstate->StateID) {
00395                         pSearchStateSpace->G = state->g;
00396                         //minkey.key[0] = 
00397                         //printf("search exited with a solution for eps=%.3f\n", pSearchStateSpace->eps);
00398                         //printf("eps=%f time_elapsed=%.3f\n", pSearchStateSpace->eps, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
00399                 
00400                         searchexpands += expands;
00401                         return 1;// so that it does not declare it as run out of time
00402                         
00403                 }
00404 
00405 
00406                 //double e_val = floor(minkey.key[0]*100.0) / 100.0;
00407                 double e_val = minkey.key[0];
00408                 //double save = pSearchStateSpace->eps;
00409                 if(e_val < pSearchStateSpace->eps) { // && e_val>=ana_FINAL_EPS
00410                         pSearchStateSpace->eps = minkey.key[0];
00411                         //if(save - e_val > 0.01)  
00412                         //printf("eps=%f time_elapsed=%.6f\n", pSearchStateSpace->eps, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
00413                 }
00414                 
00415                 
00416                 
00417 
00418 
00419 #if DEBUG
00420                 //fprintf(fDeb, "expanding state(%d): h=%d g=%u key=%u v=%u iterc=%d callnuma=%d expands=%d (g(goal)=%u)\n",
00421                 //      state->MDPstate->StateID, state->h, state->g, state->g+(int)(pSearchStateSpace->eps*state->h), state->v, 
00422                 //      state->iterationclosed, state->callnumberaccessed, state->numofexpands, searchgoalstate->g);
00423                 //fprintf(fDeb, "expanding: ");
00424                 //PrintSearchState(state, fDeb);
00425                 if(state->listelem[ana_INCONS_LIST_ID]  != NULL)
00426                 {
00427                         fprintf(fDeb, "ERROR: expanding a state from inconslist\n");
00428                         printf("ERROR: expanding a state from inconslist\n");
00429                         exit(1);
00430                 }
00431                 //fflush(fDeb);
00432 #endif
00433 
00434 #if DEBUG
00435                 if(minkey.key[0] < oldkey.key[0] && fabs(this->finitial_eps - 1.0) < ERR_EPS)
00436                 {
00437                         //printf("WARN in search: the sequence of keys decreases\n");
00438                         //exit(1);
00439                 }
00440                 oldkey = minkey;
00441 #endif
00442 
00443                 if(state->v == state->g)
00444                 {
00445                         printf("ERROR: consistent state is being expanded\n");
00446 #if DEBUG
00447                         fprintf(fDeb, "ERROR: consistent state is being expanded\n");
00448                         exit(1);
00449 #endif
00450                 }
00451 
00452                 //recompute state value      
00453                 state->v = state->g;
00454                 state->iterationclosed = pSearchStateSpace->searchiteration;
00455 
00456                 //new expand      
00457                 expands++;
00458                 state->numofexpands++;
00459 
00460 
00461                 if(bforwardsearch == false)
00462                         UpdatePreds(state, pSearchStateSpace);
00463                 else
00464                         UpdateSuccs(state, pSearchStateSpace);
00465                 
00466                 //recompute minkey
00467                 minkey.key[0] = -(pSearchStateSpace->heap->getminkeyheap().key[0]);
00468 
00469                 //recompute goalkey if necessary
00470                 
00471                 pSearchStateSpace->G = searchgoalstate->g;
00472                 
00473                 if(expands%100000 == 0 && expands > 0)
00474                 {
00475                         //printf("expands so far=%u\n", expands);
00476                 }
00477 
00478                 /*if(state->MDPstate->StateID == searchgoalstate->MDPstate->StateID) {
00479                         goalkey.key[0] = minkey.key[0];
00480                         break;
00481                 }*/
00482         }
00483         
00484         int retv = 1;
00485         if(searchgoalstate->g == INFINITECOST && pSearchStateSpace->heap->emptyheap())
00486         {
00487                 printf("solution does not exist: search exited because heap is empty\n");
00488                 retv = 0;
00489         }
00490         else if(!pSearchStateSpace->heap->emptyheap() && 0 < minkey.key[0])
00491         {
00492                 printf("search exited because it ran out of time\n");
00493                 //printf("Goalkey=%f and minkey=%f", goalkey.key[0], minkey.key[0]);
00494                 retv = 2;
00495         }
00496         else if(searchgoalstate->g == INFINITECOST && !pSearchStateSpace->heap->emptyheap())
00497         {
00498                 printf("solution does not exist: search exited because all candidates for expansion have infinite heuristics\n");
00499                 retv = 0;
00500         }
00501         else
00502         {
00503                 //printf("eps=%.3f time=%.3f\n", pSearchStateSpace->eps, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
00504                 retv = 3;
00505         }
00506 
00507         //fprintf(fDeb, "expanded=%d\n", expands);
00508 
00509         searchexpands += expands;
00510 
00511         return retv;            
00512 }
00513 
00514 
00515 
00516 void anaPlanner::Reevaluatefvals(anaSearchStateSpace_t* pSearchStateSpace)
00517 {
00518         CKey key;
00519         int i;
00520         CHeap* pheap = pSearchStateSpace->heap;
00521         
00522         //recompute priorities for states in OPEN and reorder it
00523         for (i = 1; i <= pheap->currentsize; ++i)
00524           {
00525             //anaState* state = (anaState*)pheap->heap[i].heapstate;
00526 
00527                 // CHANGED - cast removed
00528 
00529             pheap->heap[i].key.key[0] = (long)-get_e_value(pSearchStateSpace, ((anaState*) pheap->heap[i].heapstate)->MDPstate->StateID); 
00530             
00531                 
00532                 //pheap->heap[i].key.key[1] = state->h; 
00533           }
00534         pheap->makeheap();
00535 
00536         pSearchStateSpace->bReevaluatefvals = false;
00537 }
00538 
00539 
00540 
00541 
00542 //creates (allocates memory) search state space
00543 //does not initialize search statespace
00544 int anaPlanner::CreateSearchStateSpace(anaSearchStateSpace_t* pSearchStateSpace)
00545 {
00546 
00547         //create a heap
00548         pSearchStateSpace->heap = new CHeap;
00549         //pSearchStateSpace->inconslist = new CHeap;
00550         MaxMemoryCounter += sizeof(CHeap);
00551         MaxMemoryCounter += sizeof(CList);
00552 
00553         pSearchStateSpace->searchgoalstate = NULL;
00554         pSearchStateSpace->searchstartstate = NULL;
00555 
00556         searchexpands = 0;
00557 
00558 
00559     pSearchStateSpace->bReinitializeSearchStateSpace = false;
00560         
00561         return 1;
00562 }
00563 
00564 //deallocates memory used by SearchStateSpace
00565 void anaPlanner::DeleteSearchStateSpace(anaSearchStateSpace_t* pSearchStateSpace)
00566 {
00567         if(pSearchStateSpace->heap != NULL)
00568         {
00569                 pSearchStateSpace->heap->makeemptyheap();
00570                 delete pSearchStateSpace->heap;
00571                 pSearchStateSpace->heap = NULL;
00572         }
00573 
00574         /*if(pSearchStateSpace->inconslist != NULL)
00575         {
00576                 pSearchStateSpace->inconslist->makeemptyheap();
00577                 delete pSearchStateSpace->inconslist;
00578                 pSearchStateSpace->inconslist = NULL;
00579         }
00580 */
00581         //delete the states themselves
00582         int iend = (int)pSearchStateSpace->searchMDP.StateArray.size();
00583         for(int i=0; i < iend; i++)
00584         {
00585                 CMDPSTATE* state = pSearchStateSpace->searchMDP.StateArray[i];
00586                 if(state != NULL && state->PlannerSpecificData != NULL) {
00587                         DeleteSearchStateData((anaState*)state->PlannerSpecificData);
00588                         free((anaState*)state->PlannerSpecificData);
00589                           state->PlannerSpecificData = NULL;
00590                 }
00591         }
00592         pSearchStateSpace->searchMDP.Delete();
00593 }
00594 
00595 
00596 
00597 //reset properly search state space
00598 //needs to be done before deleting states
00599 int anaPlanner::ResetSearchStateSpace(anaSearchStateSpace_t* pSearchStateSpace)
00600 {
00601         pSearchStateSpace->heap->makeemptyheap();
00602 //      pSearchStateSpace->inconslist->makeemptyheap();
00603 
00604         return 1;
00605 }
00606 
00607 //initialization before each search
00608 void anaPlanner::ReInitializeSearchStateSpace(anaSearchStateSpace_t* pSearchStateSpace)
00609 {
00610         CKey key;
00611 
00612         //increase callnumber
00613         pSearchStateSpace->callnumber++;
00614 
00615         //reset iteration
00616         pSearchStateSpace->searchiteration = 0;
00617         pSearchStateSpace->bNewSearchIteration = true;
00618         pSearchStateSpace->G = INFINITECOST;
00619 
00620 #if DEBUG
00621     fprintf(fDeb, "reinitializing search state-space (new call number=%d search iter=%d)\n", 
00622             pSearchStateSpace->callnumber,pSearchStateSpace->searchiteration );
00623 #endif
00624 
00625 
00626 
00627         pSearchStateSpace->heap->makeemptyheap();
00628         //pSearchStateSpace->inconslist->makeemptyheap();
00629     //reset 
00630         pSearchStateSpace->eps = this->finitial_eps;
00631     pSearchStateSpace->eps_satisfied = INFINITECOST;
00632 
00633         //initialize start state
00634         anaState* startstateinfo = (anaState*)(pSearchStateSpace->searchstartstate->PlannerSpecificData);
00635         if(startstateinfo->callnumberaccessed != pSearchStateSpace->callnumber)
00636                 ReInitializeSearchStateInfo(startstateinfo, pSearchStateSpace);
00637 
00638         startstateinfo->g = 0;
00639 
00640         //insert start state into the heap
00641 
00642 
00643         key.key[0] = (long)-get_e_value(pSearchStateSpace, startstateinfo->MDPstate->StateID); // CHANGED - long int cast removed
00644         
00645         
00646         
00647         //key.key[1] = startstateinfo->h;
00648         pSearchStateSpace->heap->insertheap(startstateinfo, key);
00649 
00650     pSearchStateSpace->bReinitializeSearchStateSpace = false;
00651         pSearchStateSpace->bReevaluatefvals = false;
00652 }
00653 
00654 //very first initialization
00655 int anaPlanner::InitializeSearchStateSpace(anaSearchStateSpace_t* pSearchStateSpace)
00656 {
00657 
00658         if(pSearchStateSpace->heap->currentsize != 0)
00659         {
00660                 SBPL_ERROR("ERROR in InitializeSearchStateSpace: heap or list is not empty\n");
00661                 throw new SBPL_Exception();
00662         }
00663 
00664         pSearchStateSpace->eps = this->finitial_eps;
00665     pSearchStateSpace->eps_satisfied = INFINITECOST;
00666         pSearchStateSpace->searchiteration = 0;
00667         pSearchStateSpace->bNewSearchIteration = true;
00668         pSearchStateSpace->callnumber = 0;
00669         pSearchStateSpace->bReevaluatefvals = false;
00670 
00671 
00672 
00673 
00674         pSearchStateSpace->G=INFINITECOST;
00675 
00676         //create and set the search start state
00677         pSearchStateSpace->searchgoalstate = NULL;
00678         //pSearchStateSpace->searchstartstate = GetState(SearchStartStateID, pSearchStateSpace);
00679     pSearchStateSpace->searchstartstate = NULL;
00680         
00681 
00682     pSearchStateSpace->bReinitializeSearchStateSpace = true;
00683 
00684         return 1;
00685 
00686 }
00687 
00688 
00689 int anaPlanner::SetSearchGoalState(int SearchGoalStateID, anaSearchStateSpace_t* pSearchStateSpace)
00690 {
00691         if(pSearchStateSpace->searchgoalstate == NULL || 
00692                 pSearchStateSpace->searchgoalstate->StateID != SearchGoalStateID)
00693         {
00694                 pSearchStateSpace->searchgoalstate = GetState(SearchGoalStateID, pSearchStateSpace);
00695 
00696                 //should be new search iteration
00697                 pSearchStateSpace->eps_satisfied = INFINITECOST;
00698                 pSearchStateSpace->bNewSearchIteration = true;
00699                 pSearchStateSpace_->eps = this->finitial_eps;
00700 
00701 
00702                 //recompute heuristic for the heap if heuristics is used
00703 #if USE_HEUR
00704                 for(int i = 0; i < (int)pSearchStateSpace->searchMDP.StateArray.size(); i++)
00705                 {
00706                         CMDPSTATE* MDPstate = pSearchStateSpace->searchMDP.StateArray[i];
00707                         anaState* state = (anaState*)MDPstate->PlannerSpecificData;
00708                         state->h = ComputeHeuristic(MDPstate, pSearchStateSpace);
00709                 }
00710                 
00711                 pSearchStateSpace->bReevaluatefvals = true;
00712 #endif
00713         }
00714 
00715 
00716         return 1;
00717 
00718 }
00719 
00720 
00721 int anaPlanner::SetSearchStartState(int SearchStartStateID, anaSearchStateSpace_t* pSearchStateSpace)
00722 {
00723 
00724         CMDPSTATE* MDPstate = GetState(SearchStartStateID, pSearchStateSpace);
00725 
00726         if(MDPstate !=  pSearchStateSpace->searchstartstate)
00727         {       
00728                 pSearchStateSpace->searchstartstate = MDPstate;
00729                 pSearchStateSpace->bReinitializeSearchStateSpace = true;
00730         }
00731 
00732         return 1;
00733 
00734 }
00735 
00736 
00737 
00738 int anaPlanner::ReconstructPath(anaSearchStateSpace_t* pSearchStateSpace)
00739 {       
00740 
00741 
00742         if(bforwardsearch) //nothing to do, if search is backward
00743         {
00744                 CMDPSTATE* MDPstate = pSearchStateSpace->searchgoalstate;
00745                 CMDPSTATE* PredMDPstate;
00746                 anaState *predstateinfo, *stateinfo;
00747 
00748 
00749 
00750 #if DEBUG
00751                 fprintf(fDeb, "reconstructing a path:\n");
00752 #endif
00753 
00754                 while(MDPstate != pSearchStateSpace->searchstartstate)
00755                 {
00756                         stateinfo = (anaState*)MDPstate->PlannerSpecificData;
00757 
00758 #if DEBUG
00759                         PrintSearchState(stateinfo, fDeb);
00760 #endif
00761                         if(stateinfo->g == INFINITECOST)
00762                         {       
00763                                 //printf("ERROR in ReconstructPath: g of the state on the path is INFINITE\n");
00764                                 //exit(1);
00765                                 return -1;
00766                         }
00767 
00768                         if(stateinfo->bestpredstate == NULL)
00769                         {
00770                                 SBPL_ERROR("ERROR in ReconstructPath: bestpred is NULL\n");
00771                                 throw new SBPL_Exception();
00772 
00773                         }
00774 
00775                         //get the parent state
00776                         PredMDPstate = stateinfo->bestpredstate;
00777                         predstateinfo = (anaState*)PredMDPstate->PlannerSpecificData;
00778 
00779                         //set its best next info
00780                         predstateinfo->bestnextstate = MDPstate;
00781 
00782                         //check the decrease of g-values along the path
00783                         if(predstateinfo->v >= stateinfo->g)
00784                         {
00785                                 SBPL_ERROR("ERROR in ReconstructPath: g-values are non-decreasing\n");
00786                                 PrintSearchState(predstateinfo, fDeb);
00787                                 throw new SBPL_Exception();
00788 
00789                         }
00790 
00791                         //transition back
00792                         MDPstate = PredMDPstate;
00793                 }
00794         }
00795 
00796         return 1;
00797 }
00798 
00799 
00800 
00801 void anaPlanner::PrintSearchPath(anaSearchStateSpace_t* pSearchStateSpace, FILE* fOut)
00802 {
00803         anaState* searchstateinfo;
00804         CMDPSTATE* state;
00805         int goalID;
00806         int PathCost;
00807 
00808         if(bforwardsearch)
00809         {
00810                 state  = pSearchStateSpace->searchstartstate;
00811                 goalID = pSearchStateSpace->searchgoalstate->StateID;
00812         }
00813         else
00814         {
00815                 state = pSearchStateSpace->searchgoalstate;
00816                 goalID = pSearchStateSpace->searchstartstate->StateID;
00817         }
00818         if(fOut == NULL)
00819                 fOut = stdout;
00820 
00821         PathCost = ((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g;
00822 
00823         fprintf(fOut, "Printing a path from state %d to the goal state %d\n", 
00824                         state->StateID, pSearchStateSpace->searchgoalstate->StateID);
00825         fprintf(fOut, "Path cost = %d:\n", PathCost);
00826                         
00827         
00828         environment_->PrintState(state->StateID, false, fOut);
00829 
00830         int costFromStart = 0;
00831         while(state->StateID != goalID)
00832         {
00833                 fprintf(fOut, "state %d ", state->StateID);
00834 
00835                 if(state->PlannerSpecificData == NULL)
00836                 {
00837                         fprintf(fOut, "path does not exist since search data does not exist\n");
00838                         break;
00839                 }
00840 
00841                 searchstateinfo = (anaState*)state->PlannerSpecificData;
00842 
00843                 if(searchstateinfo->bestnextstate == NULL)
00844                 {
00845                         fprintf(fOut, "path does not exist since bestnextstate == NULL\n");
00846                         break;
00847                 }
00848                 if(searchstateinfo->g == INFINITECOST)
00849                 {
00850                         fprintf(fOut, "path does not exist since bestnextstate == NULL\n");
00851                         break;
00852                 }
00853 
00854                 int costToGoal = PathCost - costFromStart;
00855                 int transcost = searchstateinfo->g - ((anaState*)(searchstateinfo->bestnextstate->PlannerSpecificData))->v;
00856                 if(bforwardsearch)
00857                         transcost = -transcost;
00858 
00859                 costFromStart += transcost;
00860 
00861                 fprintf(fOut, "g=%d-->state %d, h = %d ctg = %d  ", searchstateinfo->g,                         
00862                         searchstateinfo->bestnextstate->StateID, searchstateinfo->h, costToGoal);
00863 
00864                 state = searchstateinfo->bestnextstate;
00865 
00866                 environment_->PrintState(state->StateID, false, fOut);
00867 
00868 
00869 
00870         }
00871 }
00872 
00873 void anaPlanner::PrintSearchState(anaState* state, FILE* fOut)
00874 {
00875         fprintf(fOut, "state %d: h=%d g=%u v=%u iterc=%d callnuma=%d expands=%d heapind=%d inconslist=%d\n",
00876                 state->MDPstate->StateID, state->h, state->g, state->v, 
00877                 state->iterationclosed, state->callnumberaccessed, state->numofexpands, state->heapindex, state->listelem[ana_INCONS_LIST_ID]?1:0);
00878         environment_->PrintState(state->MDPstate->StateID, true, fOut);
00879 
00880 }
00881 
00882 
00883 
00884 int anaPlanner::getHeurValue(anaSearchStateSpace_t* pSearchStateSpace, int StateID)
00885 {
00886         CMDPSTATE* MDPstate = GetState(StateID, pSearchStateSpace);
00887         anaState* searchstateinfo = (anaState*)MDPstate->PlannerSpecificData;
00888         return searchstateinfo->h;
00889 }
00890 
00891 
00892 vector<int> anaPlanner::GetSearchPath(anaSearchStateSpace_t* pSearchStateSpace, int& solcost)
00893 {
00894   vector<int> SuccIDV;
00895   vector<int> CostV;
00896   vector<int> wholePathIds;
00897   anaState* searchstateinfo;
00898   CMDPSTATE* state = NULL; 
00899   CMDPSTATE* goalstate = NULL;
00900   CMDPSTATE* startstate=NULL;
00901   
00902   if(bforwardsearch)
00903     {   
00904       startstate = pSearchStateSpace->searchstartstate;
00905       goalstate = pSearchStateSpace->searchgoalstate;
00906       
00907       //reconstruct the path by setting bestnextstate pointers appropriately
00908       ReconstructPath(pSearchStateSpace);
00909     }
00910   else
00911     {
00912       startstate = pSearchStateSpace->searchgoalstate;
00913       goalstate = pSearchStateSpace->searchstartstate;
00914     }
00915   
00916   
00917   state = startstate;
00918   
00919   wholePathIds.push_back(state->StateID);
00920   solcost = 0;
00921   
00922   FILE* fOut = stdout;
00923   while(state->StateID != goalstate->StateID)
00924     {
00925       if(state->PlannerSpecificData == NULL)
00926         {
00927           fprintf(fOut, "path does not exist since search data does not exist\n");
00928           break;
00929         }
00930       
00931       searchstateinfo = (anaState*)state->PlannerSpecificData;
00932       
00933       if(searchstateinfo->bestnextstate == NULL)
00934         {
00935           fprintf(fOut, "path does not exist since bestnextstate == NULL\n");
00936           break;
00937         }
00938       if(searchstateinfo->g == INFINITECOST)
00939         {
00940           fprintf(fOut, "path does not exist since bestnextstate == NULL\n");
00941           break;
00942         }
00943       
00944       environment_->GetSuccs(state->StateID, &SuccIDV, &CostV);
00945       int actioncost = INFINITECOST;
00946       for(int i = 0; i < (int)SuccIDV.size(); i++)
00947         {   
00948           
00949           if(SuccIDV.at(i) == searchstateinfo->bestnextstate->StateID)
00950             actioncost = CostV.at(i);
00951           
00952         }
00953       if(actioncost == INFINITECOST)
00954         printf("WARNING: actioncost = %d\n", actioncost);
00955       
00956       solcost += actioncost;
00957       
00958       //fprintf(fDeb, "actioncost=%d between states %d and %d\n", 
00959       //        actioncost, state->StateID, searchstateinfo->bestnextstate->StateID);
00960       //environment_->PrintState(state->StateID, false, fDeb);
00961       //environment_->PrintState(searchstateinfo->bestnextstate->StateID, false, fDeb);
00962       
00963       
00964 #if DEBUG
00965       anaState* nextstateinfo = (anaState*)(searchstateinfo->bestnextstate->PlannerSpecificData);
00966       if(actioncost != abs((int)(searchstateinfo->g - nextstateinfo->g)) && pSearchStateSpace->eps_satisfied <= 1.001)
00967         {
00968           fprintf(fDeb, "ERROR: actioncost=%d is not matching the difference in g-values of %d\n", 
00969                   actioncost, abs((int)(searchstateinfo->g - nextstateinfo->g)));
00970           printf("ERROR: actioncost=%d is not matching the difference in g-values of %d\n", 
00971                  actioncost,abs((int)(searchstateinfo->g - nextstateinfo->g)));
00972           PrintSearchState(searchstateinfo, fDeb);
00973           PrintSearchState(nextstateinfo, fDeb);
00974         }
00975 #endif
00976       
00977       
00978       state = searchstateinfo->bestnextstate;
00979       
00980       wholePathIds.push_back(state->StateID);
00981     }
00982 
00983 
00984   return wholePathIds;
00985 }
00986 
00987 
00988 
00989 bool anaPlanner::Search(anaSearchStateSpace_t* pSearchStateSpace, vector<int>& pathIds, int & PathCost, bool bFirstSolution, bool bOptimalSolution, double MaxNumofSecs) {
00990         CKey key;
00991         TimeStarted = clock();
00992     searchexpands = 0;
00993         
00994 #if DEBUG
00995         fprintf(fDeb, "new search call (call number=%d)\n", pSearchStateSpace->callnumber);
00996 #endif
00997 
00998    if(pSearchStateSpace->bReinitializeSearchStateSpace == true){
00999         //re-initialize state space 
01000         ReInitializeSearchStateSpace(pSearchStateSpace);
01001     }
01002 
01003 
01004         if(bOptimalSolution)
01005         {
01006                 pSearchStateSpace->eps = 1;
01007                 MaxNumofSecs = INFINITECOST;
01008         }
01009         else if(bFirstSolution)
01010         {
01011                 MaxNumofSecs = INFINITECOST;
01012         }
01013 
01014         //ensure heuristics are up-to-date
01015         environment_->EnsureHeuristicsUpdated((bforwardsearch==true));
01016 
01017         //the main loop of ana*
01018         int prevexpands = 0;
01019         clock_t loop_time;
01020 
01021 
01022         // CHANGE MADE TO WHILE LOOP to account for open.empty() == FALSE
01023         while(!pSearchStateSpace->heap->emptyheap() && pSearchStateSpace->eps_satisfied > ana_FINAL_EPS && (clock()- TimeStarted) < MaxNumofSecs*(double)CLOCKS_PER_SEC) {
01024        
01025                 
01026                 loop_time = clock();
01027                 //decrease eps for all subsequent iterations
01028                 /*if(fabs(pSearchStateSpace->eps_satisfied - pSearchStateSpace->eps) < ERR_EPS && !bFirstSolution)
01029                 {
01030                         pSearchStateSpace->eps = pSearchStateSpace->eps - ana_DECREASE_EPS;
01031                         if(pSearchStateSpace->eps < ana_FINAL_EPS)
01032                                 pSearchStateSpace->eps = ana_FINAL_EPS;
01033 
01034                         //the priorities need to be updated
01035                         pSearchStateSpace->bReevaluatefvals = true; 
01036 
01037                         //it will be a new search
01038                         pSearchStateSpace->bNewSearchIteration = true;
01039 
01040                         //build a new open list by merging it with incons one
01041                         BuildNewOPENList(pSearchStateSpace); 
01042 
01043                 }*/
01044 
01045 
01046 
01047                 
01048                         pSearchStateSpace->searchiteration++;
01049                         pSearchStateSpace->bNewSearchIteration = false;
01050                 
01051 
01052                 //re-compute f-values if necessary and reorder the heap
01053                 //if(pSearchStateSpace->bReevaluatefvals) 
01054                 //      Reevaluatefvals(pSearchStateSpace);
01055 
01056                 //improve or compute path
01057                 int retVal = ImprovePath(pSearchStateSpace, MaxNumofSecs);
01058                 anaState* state;
01059                 CKey key;
01060                 CHeap* open = pSearchStateSpace->heap;
01061                         //printf("states expanded: %d\t states considered: %d\t time elapsed: %f\n",searchexpands - prevexpands, pSearchStateSpace->heap->currentsize, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
01062         
01063                 double epsprime=1.0;
01064                 for(int j=1; j<=open->currentsize; ) {
01065                         state = (anaState*) open->heap[j].heapstate;
01066                         double temp_eps = (double) ((pSearchStateSpace->G*1.0) / (double) (state->g + state->h));
01067                         if(temp_eps > epsprime) {
01068                                 epsprime = temp_eps;
01069                         }
01070                         double e_val = get_e_value(pSearchStateSpace, state->MDPstate->StateID);
01071                         if(e_val <= 1.0) {
01072                                 
01073                                 open->deleteheap_unsafe(state);
01074                                 
01075                         } else {
01076                                 key.key[0]= (long)-e_val;
01077                                 
01078                                 open->updateheap_unsafe(state, key);
01079                                 ++j;
01080                         }
01081                         pSearchStateSpace->eps_satisfied = epsprime;
01082                 }
01083                 open->makeheap();                       
01084         
01085                 //print the solution cost and eps bound
01086                         if(retVal == 1) {
01087                                 //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);
01088                         
01089                         
01090                                 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);
01091                         
01092                                 //printf("states expanded: %d\t states considered: %d\t time elapsed: %f\n",searchexpands - prevexpands, pSearchStateSpace->heap->currentsize, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
01093 
01094                         }
01095 
01096 #if DEBUG
01097         fprintf(fDeb, "eps=%f expands=%d g(searchgoal)=%d time=%.3f\n", pSearchStateSpace->eps_satisfied, searchexpands - prevexpands,
01098                                                         ((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g,double(clock()-loop_time)/CLOCKS_PER_SEC);
01099                 PrintSearchState((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData, fDeb);
01100 #endif
01101                 prevexpands = searchexpands;
01102 
01103 
01104                 //if just the first solution then we are done
01105                 if(bFirstSolution)
01106                         break;
01107 
01108                 //no solution exists
01109                 if(((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g == INFINITECOST)
01110                         break;
01111 
01112         }
01113 
01114 
01115 #if DEBUG
01116         fflush(fDeb);
01117 #endif
01118 
01119         printf("Suboptimality = %.4f\n", pSearchStateSpace->eps_satisfied);
01120 
01121         PathCost = ((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g;
01122         MaxMemoryCounter += environment_->StateID2IndexMapping.size()*sizeof(int);
01123         
01124         printf("MaxMemoryCounter = %d\n", MaxMemoryCounter);
01125 
01126         int solcost = INFINITECOST;
01127         bool ret = false;
01128         if(PathCost == INFINITECOST)
01129         {
01130                 printf("could not find a solution\n");
01131                 ret = false;
01132         }
01133         else
01134         {
01135                 printf("solution is found\n");      
01136         pathIds = GetSearchPath(pSearchStateSpace, solcost);
01137         ret = true;
01138         }
01139 
01140         printf("total expands this call = %d, planning time = %.3f secs, solution cost=%d\n", 
01141            searchexpands, (clock()-TimeStarted)/((double)CLOCKS_PER_SEC), solcost);
01142     
01143 
01144     //fprintf(fStat, "%d %d\n", searchexpands, solcost);
01145 
01146         return ret;
01147 
01148 }
01149 
01150 
01151 //-----------------------------Interface function-----------------------------------------------------
01152 //returns 1 if found a solution, and 0 otherwise
01153 int anaPlanner::replan(double allocated_time_secs, vector<int>* solution_stateIDs_V)
01154 {
01155         int solcost;
01156 
01157         return replan(allocated_time_secs, solution_stateIDs_V, &solcost);
01158         
01159 }
01160 
01161 //returns 1 if found a solution, and 0 otherwise
01162 int anaPlanner::replan(double allocated_time_secs, vector<int>* solution_stateIDs_V, int* psolcost)
01163 {
01164   vector<int> pathIds; 
01165   bool bFound = false;
01166   int PathCost;
01167   //bool bFirstSolution = true;
01168   bool bFirstSolution = this->bsearchuntilfirstsolution;
01169   bool bOptimalSolution = false;
01170   *psolcost = 0;
01171   
01172   printf("planner: replan called (bFirstSol=%d, bOptSol=%d)\n", bFirstSolution, bOptimalSolution);
01173   
01174   //plan
01175   if((bFound = Search(pSearchStateSpace_, pathIds, PathCost, bFirstSolution, bOptimalSolution, allocated_time_secs)) == false) 
01176     {
01177       printf("failed to find a solution\n");
01178     }
01179   
01180   //copy the solution
01181   *solution_stateIDs_V = pathIds;
01182   *psolcost = PathCost;
01183   
01184   return (int)bFound;
01185 
01186 }
01187 
01188 
01189 int anaPlanner::set_goal(int goal_stateID)
01190 {
01191 
01192         printf("planner: setting goal to %d\n", goal_stateID);
01193         environment_->PrintState(goal_stateID, true, stdout);
01194 
01195         if(bforwardsearch)
01196         {       
01197                 if(SetSearchGoalState(goal_stateID, pSearchStateSpace_) != 1)
01198                         {
01199                                 printf("ERROR: failed to set search goal state\n");
01200                                 return 0;
01201                         }
01202         }
01203         else
01204         {
01205             if(SetSearchStartState(goal_stateID, pSearchStateSpace_) != 1)
01206         {
01207             printf("ERROR: failed to set search start state\n");
01208             return 0;
01209         }
01210         }
01211 
01212     return 1;
01213 }
01214 
01215 
01216 int anaPlanner::set_start(int start_stateID)
01217 {
01218 
01219         printf("planner: setting start to %d\n", start_stateID);
01220         environment_->PrintState(start_stateID, true, stdout);
01221 
01222         if(bforwardsearch)
01223         {       
01224 
01225             if(SetSearchStartState(start_stateID, pSearchStateSpace_) != 1)
01226         {
01227             printf("ERROR: failed to set search start state\n");
01228             return 0;
01229         }
01230         }
01231         else
01232         {
01233             if(SetSearchGoalState(start_stateID, pSearchStateSpace_) != 1)
01234         {
01235             printf("ERROR: failed to set search goal state\n");
01236             return 0;
01237         }
01238         }
01239 
01240     return 1;
01241 
01242 }
01243 
01244 
01245 
01246 void anaPlanner::costs_changed(StateChangeQuery const & stateChange)
01247 {
01248 
01249 
01250     pSearchStateSpace_->bReinitializeSearchStateSpace = true;
01251 
01252 
01253 }
01254 
01255 void anaPlanner::costs_changed()
01256 {
01257 
01258     pSearchStateSpace_->bReinitializeSearchStateSpace = true;
01259 
01260 }
01261 
01262 
01263 
01264 int anaPlanner::force_planning_from_scratch()
01265 {
01266         printf("planner: forceplanfromscratch set\n");
01267 
01268     pSearchStateSpace_->bReinitializeSearchStateSpace = true;
01269 
01270     return 1;
01271 }
01272 
01273 
01274 int anaPlanner::set_search_mode(bool bSearchUntilFirstSolution)
01275 {
01276 
01277         printf("planner: search mode set to %d\n", bSearchUntilFirstSolution);
01278 
01279         bsearchuntilfirstsolution = bSearchUntilFirstSolution;
01280 
01281         return 1;
01282 }
01283 
01284 
01285 void anaPlanner::print_searchpath(FILE* fOut)
01286 {
01287         PrintSearchPath(pSearchStateSpace_, fOut);
01288 }
01289 
01290 
01291 //---------------------------------------------------------------------------------------------------------
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


sbpl
Author(s): Maxim Likhachev/maximl@seas.upenn.edu
autogenerated on Fri Jan 18 2013 13:41:52