Sample.cpp
Go to the documentation of this file.
00001 
00007 /******************************************************************************
00008  * File: Sample.cc
00009  * Date: 30/06/2007
00010  * Author: rn
00011  * Brief: Super class of all sample classes
00012  ******************************************************************************/
00013 
00014 #include "Sample.h"
00015 #include "BeliefCache.h"
00016 #include "AlphaPlane.h"
00017 #include "AlphaPlaneMaxMeta.h"
00018 #include "GlobalResource.h"
00019 
00020 //#define DEBUGSYL_310708 1
00021 //#define DEBUGSYLTAG 1
00022 
00023 using namespace std;
00024 using namespace momdp;
00025 
00026 namespace momdp
00027 {
00028 
00029      /***************************************
00030        BASIC REACHABILITY TREE METHODS
00031       ****************************************/
00032      void Sample::samplePrepare(cacherow_stval beliefIndex_Sval)
00033      {
00034           state_val stateidx = beliefIndex_Sval.sval;
00035           int   row =  beliefIndex_Sval.row;
00036 
00037           //do the preparation work before sample
00038           BeliefTreeNode* cn = (*beliefCacheSet)[stateidx]->getRow(row)->REACHABLE;
00039           //BeliefTreeNode* cn = beliefCache->getRow(row)->REACHABLE;
00040 
00041           if(cn->isFringe())
00042           {
00043                expand(*cn);
00044           }
00045      }
00046 
00047      void Sample::samplePrepare(BeliefTreeNode* cn)
00048      {
00049           if(cn->isFringe())
00050           {
00051                expand(*cn);
00052           }
00053      }
00054 
00055      //ADD SYLTAG - this function just creates, initializes and returns pointer. It does not expand.
00056      BeliefForest* Sample::getGlobalNode(void) 
00057      {
00058           return beliefForest;
00059      }
00060 
00061 
00062 
00063 
00064      BeliefTreeNode* Sample::getNode(SharedPointer<BeliefWithState>& b_s) 
00065      {
00066           SharedPointer<belief_vector>& s = b_s->bvec;
00067           state_val stateidx = b_s->sval;
00068 
00069           bool keepLowerBound = true;
00070 
00071           DEBUG_TRACE( cout << "Sample::getNode stateidx " << stateidx; );
00072           DEBUG_TRACE( cout << " s" << endl; );
00073           DEBUG_TRACE( s->write(cout) << endl; );
00074           DEBUG_TRACE( cout << " hash: " << s->md5HashValue() << endl; );
00075 
00076           int row = (*beliefCacheSet)[stateidx]->getBeliefRowIndex(s);
00077 
00078           DEBUG_TRACE( cout << "testedRowIndex: " << row << endl; );
00079 
00080 
00081           bool isTerminal;
00082           double ubVal, lbVal;
00083 
00084           if (row==-1) 
00085           {
00086                // create a new fringe node
00087                BeliefTreeNode* cn = new BeliefTreeNode();
00088                int timeStamp = -1;//for initializing timeStamp of the beliefNode
00089                cn->s = b_s;
00090 
00091                cn->count = 0; //for SARSOP
00092                cn->checked = false;
00093 
00094                row = (*beliefCacheSet)[stateidx]->addBeliefRowWithoutCheck(cn->s->bvec);
00095 
00096                cn->cacheIndex.row = row;
00097                cn->cacheIndex.sval = cn->s->sval;
00098 
00099                isTerminal = problem->getIsTerminalState(*b_s);
00100 
00101                (*beliefCacheSet)[stateidx]->getRow( row)->REACHABLE = cn;
00102                // TODO:: check if anyone using this: solver->beliefCacheSet[stateidx]->getRow( row)->IS_TERMINAL = isTerminal;
00103 
00104                for(int i = 0 ; i < onGetNode.size(); i++)
00105                {
00106                     (*onGetNode[i])(solver, cn, b_s);
00107                }
00108 
00109 
00110 
00111                DEBUG_TRACE( cout << "cn->cacheIndex.row " << cn->cacheIndex.row << " count " << cn->count << endl; );
00112 
00113 
00114 
00115                return cn;
00116           }
00117           else 
00118           {
00119                // return existing node
00120                BeliefTreeNode* temp = (*beliefCacheSet)[stateidx]->getRow(row)->REACHABLE;
00121                return temp;
00122           }
00123      }
00124 
00125      //Assumption: cn.depth is defined already
00126      void Sample::expand(BeliefTreeNode& cn)
00127      {
00128           DEBUG_TRACE( cout<<"expand"<< endl; );
00129           DEBUG_TRACE( cout << "cn.s sval " << cn.s->sval << " index " << cn.cacheIndex.row << endl; );
00130           DEBUG_TRACE( cn.s->bvec->write(cout) << endl; );
00131 
00132           // set up successors for this fringe node (possibly creating new fringe nodes)
00133           obs_prob_vector  opv; // outcome_prob_vector opv;
00134           //state_vector *sp = new state_vector;
00135           SharedPointer<BeliefWithState> sp; // state_vector sp;
00136           obsState_prob_vector spv;  // outcome probability for values of observed state
00137           SharedPointer<SparseVector> jspv (new SparseVector());
00138 
00139           //obs_prob_vector  ospv; // outcome_prob_vector opv;
00140 
00141           cn.Q.resize(problem->getNumActions());
00142 
00143           // list<int>::iterator iter;          doesnt seem to be used
00144 
00145           for(Actions::iterator aIter =  problem->actions->begin() ; aIter != problem->actions->end(); aIter ++)
00146           {
00147                int a = aIter.index();
00148                DEBUG_TRACE( cout << "Sample::expand a " << a << endl; );
00149                BeliefTreeQEntry& Qa = cn.Q[a];
00150                //Qa.immediateReward = problem->getReward(cn.s, a);
00151                Qa.immediateReward = problem->rewards->getReward(*cn.s, a);
00152 
00153 
00154                problem->getObsStateProbVector(spv, *(cn.s), a); // P(Xn|cn.s,a)
00155                Qa.stateOutcomes.resize(spv.size());
00156                Qa.valid = true;
00157 
00158 
00159 
00160                DEBUG_TRACE( cout << "spv" << endl; );
00161                DEBUG_TRACE( spv.write(cout) << endl; );
00162 
00163                for(States::iterator xIter =  problem->XStates->begin() ; xIter != problem->XStates->end(); xIter ++)
00164                {
00165                     int Xn = xIter.index();
00166 
00167                     DEBUG_TRACE( cout << "Sample::expand Xn " << Xn << endl; );
00168 
00169                     double sprob = spv(Xn);
00170                     if (sprob > OBS_IS_ZERO_EPS) 
00171                     {
00172                          BeliefTreeObsState* xe = new BeliefTreeObsState();
00173                          Qa.stateOutcomes[Xn] = xe;
00174                          problem->getJointUnobsStateProbVector(*jspv, (SharedPointer<BeliefWithState> )cn.s, a, Xn);
00175                          //problem->getStatenObsProbVectorFast(ospv, a, Xn, jspv);
00176                          problem->getObsProbVectorFast(opv, a, Xn, *jspv); // only the joint prob is useful for later but we calculate the observation prob P(o|Xn,cn.s,a)
00177                          //problem->getObsProbVector(opv, cn.s, a, Xn);
00178                          xe->outcomes.resize(opv.size());
00179 
00180                          for(Observations::iterator oIter =  problem->observations->begin() ; oIter != problem->observations->end(); oIter ++)
00181                          {
00182                               //FOR(o, opv.size()) 
00183                               int o = oIter.index();
00184 
00185                               DEBUG_TRACE( cout << "Sample::expand o " << o << endl; );
00186 
00187                               double oprob = opv(o);
00188                               if (oprob > OBS_IS_ZERO_EPS) 
00189                               {
00190                                    BeliefTreeEdge* e = new BeliefTreeEdge();
00191                                    xe->outcomes[o] = e;
00192                                    //QaXn.outcomes[o] = e;
00193                                    //e->obsProb = oprob;
00194                                    e->obsProb = oprob * sprob; // P(o,Xn|cn.s,a) = P(Xn|cn.s,a) * P(o|Xn,cn.s,a)
00195                                    //e->nextState = getNode(problem->getNextBeliefStvalFast(sp, a, o, Xn, jspv));
00196                                    sp = (problem->beliefTransition->nextBelief2(cn.s, a, o, Xn, jspv)) ;
00197                                    sp->bvec->finalize();
00198                                    e->nextState = getNode(sp);
00199 
00200                                    DEBUG_TRACE( cout << "Sample::expand e->nextState row " << e->nextState->cacheIndex.row << endl; );
00201                                    DEBUG_TRACE( cout << "Sample::expand e->nextState belief " << endl; );
00202                                    DEBUG_TRACE( e->nextState->s->bvec->write(cout) << endl; );
00203 
00204                                    //e->nextState = getNode(problem->getNextBeliefStval(sp, cn.s, a, o, Xn));
00205                                    e->nextState->count++;//increment valid-path count, for the current
00206 
00207                                    DEBUG_TRACE( cout << "e->nextState->cacheIndex.row " << e->nextState->cacheIndex.row << " count " << e->nextState->count << endl; );
00208                                    // new path which runs into the node
00209                               }
00210                               else 
00211                               {
00212                                    xe->outcomes[o] = NULL;
00213                               }
00214                          }
00215                     } 
00216                     else 
00217                     { 
00218                          Qa.stateOutcomes[Xn] = NULL;
00219                     }
00220                }
00221                Qa.ubVal = CB_QVAL_UNDEFINED;
00222           }
00223           numStatesExpanded++;
00224      }
00225 
00226 };


appl
Author(s): petercai
autogenerated on Tue Jan 7 2014 11:02:29