CoLoc.cpp
Go to the documentation of this file.
00001 #include "CoLoc.h"
00002 #include <string>
00003 #include "ProblemParams.h"
00004 #include "VariableRelation.h"
00005 #include "Move2DVarRel.h"
00006 #include "InRangeAllNto1VarRel.h"
00007 #include "NoisyVariableRel.h"
00008 #include "VariableCombined.h"
00009 #include "BooleanVariable.h"
00010 
00011 #include "Move2DPathFollowerActPosHasGuideNoisyVarRel.h"
00012 #include "Obs2DBeaconNoisy.h"
00013 #include "RewardMovementCostRel.h"
00014 #include "RewardOnPath.h"
00015 #include "RewardAtGoal.h"
00016 #include "TriggerVarRel.h"
00017 #include "TerminationVarPaddedRel.h"
00018 #include "StateTransitionXByFunction.h"
00019 #include "StateTransitionXYByFunction.h"
00020 #include "RewardsByFunction.h"
00021 #include "ObservationProbabilitiesByFunction.h"
00022 
00023 using namespace std;
00024 
00025 
00026 CoLoc::CoLoc(void) : MOMDP()
00027 {
00028         // done by MOMDP constructor
00029         //beliefTransition = new BeliefTransitionMOMDP();
00030         //beliefTransition->problem = this;
00031 
00032         //
00033         //XStates = new States();
00034         //YStates = new States();
00035         //actions = new Actions();
00036         //observations = new Observations();
00037 
00038         
00039         StateTransitionXByFunction* XTransFunctions  =new StateTransitionXByFunction();
00040         XTransFunctions->problem = this;
00041         StateTransitionXYByFunction* XYTransFunctions  =new StateTransitionXYByFunction();
00042         XYTransFunctions->problem = this;
00043         RewardsByFunction* RewardsFunctions = new RewardsByFunction();
00044         RewardsFunctions->problem = this;
00045         ObservationProbabilitiesByFunction* ObsFunctions = new ObservationProbabilitiesByFunction();
00046         ObsFunctions->problem = this;
00047 
00048         this->XTrans = XTransFunctions;
00049         this->XYTrans = XYTransFunctions;
00050         this->obsProb = ObsFunctions;
00051         this->rewards = RewardsFunctions;
00052 
00053         //pomdpR = NULL; pomdpT = NULL; pomdpTtr = NULL; pomdpO = NULL;
00054 
00055         SharedPointer<ProblemParams> params(new ProblemParams());
00056         baseMap.setBounds(3,3);
00057         
00058         discount = params->DiscountFactor;
00059 
00060         SharedPointer<BooleanVariable> auvGetsGPSPos (new BooleanVariable("auvGetsGPSPos", 1.0, 0.0));
00061     SharedPointer<BooleanVariable> termVar (new BooleanVariable("termVar", 0.0, 1.0));
00062 
00063         SharedPointer<Map2DPosVar> suv1Pos = baseMap.makePosVar("suv1Pos");
00064     SharedPointer<Map2DPosVar> suv2Pos = baseMap.makePosVar("suv2Pos");
00065     SharedPointer<Map2DPosVar> auvPos = baseMap.makePosVar("auvPos");
00066 
00067         SharedPointer<Map2DPosValue> suv1InitPos = dynamic_pointer_cast<Map2DPosValue>(suv1Pos->getValueByName("x0y0"));
00068         SharedPointer<Map2DPosValue> suv2InitPos = dynamic_pointer_cast<Map2DPosValue>(suv2Pos->getValueByName("x0y2"));
00069 
00070     suv1Pos->setInitPos(suv1InitPos);
00071     suv2Pos->setInitPos(suv2InitPos);
00072         SharedPointer<Map2DPosValue> auvInitPos = dynamic_pointer_cast<Map2DPosValue>(auvPos->getValueByName("x0y1"));
00073         auvPos->setInitPos(auvInitPos);
00074 
00075 
00076         SharedPointer<Map2DPosValue> step1 = dynamic_pointer_cast<Map2DPosValue>(auvPos->getValueByName("x0y1"));
00077         SharedPointer<Map2DPosValue> step2 = dynamic_pointer_cast<Map2DPosValue>(auvPos->getValueByName("x1y1"));
00078         SharedPointer<Map2DPosValue> step3 = dynamic_pointer_cast<Map2DPosValue>(auvPos->getValueByName("x2y1"));
00079 
00080         baseMapPath = SharedPointer<Map2DPath> (new Map2DPath());
00081         baseMapPath->addStep(step1);
00082         baseMapPath->addStep(step2);
00083         baseMapPath->addStep(step3);
00084 
00085     SharedPointer<Variable> suv1Action (new Variable("suv1Action"));
00086     suv1Action->addValue("up");
00087     suv1Action->addValue("down");
00088     suv1Action->addValue("left");
00089     suv1Action->addValue("right");
00090     suv1Action->addValue("noop");
00091 
00092     SharedPointer<Variable>  suv2Action (new Variable("suv2Action"));
00093     suv2Action->addValue("up");
00094     suv2Action->addValue("down");
00095     suv2Action->addValue("left");
00096     suv2Action->addValue("right");
00097     suv2Action->addValue("noop");
00098 
00099     SharedPointer<Move2DVarRel> suv1Move (new Move2DVarRel(suv1Action, suv1Pos));
00100         SharedPointer<Move2DVarRel> suv2Move (new Move2DVarRel(suv2Action, suv2Pos));
00101 
00102         
00103     SharedPointer<InRangeAllNto1VarRel> auvGetsGPSPosRel (new InRangeAllNto1VarRel(auvGetsGPSPos, auvPos, params->ModemRange));
00104     auvGetsGPSPosRel->addSrcVar(suv1Pos);
00105     auvGetsGPSPosRel->addSrcVar(suv2Pos);
00106         
00107         SharedPointer<NoisyVariableRel> auvGetsGPSPosRelNoisy (new  NoisyVariableRel(auvGetsGPSPosRel, auvGetsGPSPos->getTrueValue() , params->SUVtoAUVProb, auvGetsGPSPos->getFalseValue()));
00108 
00109     SharedPointer<Move2DPathFollowerActPosHasGuideNoisyVarRel> auvMov ( new Move2DPathFollowerActPosHasGuideNoisyVarRel(auvPos, auvGetsGPSPos, baseMapPath, params));
00110 
00111         
00112     // Observation Variables
00113     SharedPointer<BooleanVariable> obsAuvGetsGPSPos (new BooleanVariable("obsAuvPosCal", 1.0, 0.0));
00114                  
00115     SharedPointer<Map2DPosVar> obsAuvPos = baseMap.makePosVar("obsAuvPos");
00116     SharedPointer<VariableCombined> obsAuvPosWithNAValue ( new VariableCombined("obsAuvPosWithNAValue") );
00117     obsAuvPosWithNAValue->addSubVar(dynamic_pointer_cast<IVariable>(obsAuvPos));
00118     obsAuvPosWithNAValue->addSubVar(obsAuvGetsGPSPos);
00119     obsAuvPosWithNAValue->seal();
00120     
00121     //SharedPointer<Variable> obsAuvPosWithNAValue (new Variable("obsAuvPosWithNAValue"));
00122         
00123     obsAuvPosWithNAValue->addValue("na");
00124 
00125     SharedPointer<Obs2DBeaconNoisy> obsAuvPosRel (new Obs2DBeaconNoisy(obsAuvPosWithNAValue, suv1Pos, suv2Pos, auvPos, auvGetsGPSPos, params));
00126 
00127                 
00128     SharedPointer<Variable> movementCost1 (new Variable("movementCost1"));
00129     SharedPointer<RewardMovementCostRel> movementCostRel1 (new RewardMovementCostRel(movementCost1, suv1Action, params));
00130     SharedPointer<Variable> movementCost2 (new Variable("movementCost2"));
00131     SharedPointer<RewardMovementCostRel> movementCostRel2 (new RewardMovementCostRel(movementCost2, suv2Action, params));
00132 
00133                 
00134     SharedPointer<Variable> auvOnPathReward (new Variable("auvOnPathReward"));
00135     SharedPointer<RewardOnPath> auvOnPathRewardRel ( new RewardOnPath(auvOnPathReward, auvPos, baseMapPath, params) );
00136 
00137     SharedPointer<Variable> auvAtGoalReward (new Variable("auvAtGoalReward"));
00138     SharedPointer<RewardAtGoal> auvAtGoalRewardRel ( new RewardAtGoal(auvAtGoalReward, auvPos, baseMapPath, params) );
00139         
00140 
00141 
00142     // States
00143         this->XStates->vars.push_back(suv1Pos);
00144     this->XStates->vars.push_back(suv2Pos);
00145     this->XStates->vars.push_back(termVar);
00146     this->YStates->vars.push_back(auvPos);
00147     this->YStates->vars.push_back(auvGetsGPSPos);
00148    
00149     // Actions
00150         this->actions->vars.push_back(suv1Action);
00151     this->actions->vars.push_back(suv2Action);
00152 
00153     // Observations
00154     this->observations->vars.push_back(obsAuvPosWithNAValue);
00155 
00156         
00157     // Rewards
00158         this->rewards->vars.push_back(movementCost1);
00159     this->rewards->vars.push_back(movementCost2);
00160     this->rewards->vars.push_back(auvOnPathReward);
00161     this->rewards->vars.push_back(auvAtGoalReward);
00162 
00163 
00164         this->XStates->sealAndPopulate();
00165         this->YStates->sealAndPopulate();
00166         this->actions->sealAndPopulate();
00167         this->observations->sealAndPopulate();
00168         
00169 
00170 
00171     // State Transitions
00172         
00173         int numSteps = baseMapPath->getPath().size();
00174     SharedPointer<IVariableValue> lastStepPosStr = baseMapPath->getPath()[numSteps - 1];
00175 
00176         SharedPointer<TriggerVarRel> termVarRel (new TriggerVarRel(termVar, auvPos, lastStepPosStr, termVar->getValueByName("true"), termVar->getValueByName("false") ) );
00177 
00178         SharedPointer<TerminationVarPaddedRel> suv1MovTerm (new TerminationVarPaddedRel(suv1Move, termVar));
00179         SharedPointer<TerminationVarPaddedRel> suv2MovTerm (new TerminationVarPaddedRel(suv2Move, termVar));
00180         SharedPointer<TerminationVarPaddedRel> auvMovTerm (new TerminationVarPaddedRel(auvMov, termVar));
00181         SharedPointer<TerminationVarPaddedRel> auvGetsGPSPosRelNoisyTerm (new TerminationVarPaddedRel(auvGetsGPSPosRelNoisy, termVar));
00182         SharedPointer<TerminationVarPaddedRel> termVarRelTerm (new TerminationVarPaddedRel(termVarRel, termVar));
00183 
00184 
00185 
00186 
00187         XTransFunctions->relations.push_back(suv1MovTerm);
00188         XTransFunctions->relations.push_back(suv2MovTerm);
00189         //XTransFunctions->relations.push_back(auvMovTerm);
00190         //XTransFunctions->relations.push_back(auvGetsGPSPosRelNoisyTerm);
00191         XTransFunctions->relations.push_back(termVarRelTerm);
00192         
00193         //XYTransFunctions->relations.push_back(suv1MovTerm);
00194         //XYTransFunctions->relations.push_back(suv2MovTerm);
00195         XYTransFunctions->relations.push_back(auvMovTerm);
00196         XYTransFunctions->relations.push_back(auvGetsGPSPosRelNoisyTerm);
00197         //XYTransFunctions->relations.push_back(termVarRelTerm);
00198 
00199         // Obs probs
00200         ObsFunctions->relations.push_back(obsAuvPosRel);
00201 
00202     // Rewards
00203         RewardsFunctions->relations.push_back(movementCostRel1);
00204     RewardsFunctions->relations.push_back(movementCostRel2);
00205     RewardsFunctions->relations.push_back(auvOnPathRewardRel);
00206     RewardsFunctions->relations.push_back(auvAtGoalRewardRel);
00207 
00208 
00209         copy(*this->initialBelief , *this->YStates->getInitialProb());
00210         SharedPointer<SparseVector> initX = this->XStates->getInitialProb();
00211         if(initX->data.size() > 1 )
00212         {
00213                 this->initialBeliefStval->sval = -1;
00214         }
00215         else
00216         {
00217                 this->initialBeliefStval->sval = initX->data[0].index;
00218         }
00219         copy(*this->initialBeliefStval->bvec, *initX);
00220         copy(*this->initialBeliefX ,*initX);
00221         
00222         // post-process: calculate isPOMDPTerminalState
00223 
00224         int numStatesObs = this->XStates->size();
00225         int numStatesUnobs = this->YStates->size();
00226         int numActions = this->actions->size();
00227         
00228         this->isPOMDPTerminalState.resize(numStatesObs);
00229 
00230         FOR     (state_idx, numStatesObs) 
00231         {
00232                 this->isPOMDPTerminalState[state_idx].resize(numStatesUnobs, true);
00233                 FOR (s, numStatesUnobs) 
00234                 {
00235                         FOR (a, numActions) 
00236                         {
00237                                 if (   (fabs(1.0 - (XTrans->getMatrix(a,state_idx)->operator ()(s,state_idx)))> OBS_IS_ZERO_EPS) 
00238                                         || (fabs(1.0 - (XYTrans->getMatrix(a,state_idx)->operator ()(s,s))) > OBS_IS_ZERO_EPS) 
00239                                         || (rewards->getMatrix(state_idx)->operator ()(s,a) != 0.0) 
00240                                         )
00241                                 {
00242                                         this->isPOMDPTerminalState[state_idx][s] = false;
00243                                         break;
00244                                 }
00245                         }
00246                 }
00247         }
00248         
00249 }
00250 
00251 bool CoLoc::hasPOMDPMatrices()
00252 {
00253         return false;
00254 }
00255 
00256 
00257 CoLoc::~CoLoc(void)
00258 {
00259         //delete beliefTransition;
00260 }
00261 
00262 obsState_prob_vector& CoLoc::getObsStateProbVector(obsState_prob_vector& result, BeliefWithState& b, int a)
00263 {
00264         int Xc = b.sval; // currrent value for observed state variable
00265         mult( result, *b.bvec, *this->XTrans->getMatrix(a, Xc));
00266 
00267         return result;
00268 }
00269 
00270 
00271 string CoLoc::ToString()
00272 {
00273         stringstream sb ;
00274         sb << "discount : " << discount << endl;
00275         sb << "initialBelief : " << endl;
00276         initialBelief->write(sb) << endl;
00277         sb << "initialBeliefStval : " << endl;
00278         sb << "initialBeliefStval stval: " <<  initialBeliefStval->sval << endl;
00279         initialBeliefStval->bvec->write(sb) << endl;
00280         sb << "initialBeliefX : " << endl;
00281         initialBeliefX->write(sb) << endl;
00282         sb << "Num X States : " << XStates->size() << endl;
00283         sb << "Num Y States : " << YStates->size() << endl;
00284         sb << "Num Action : " << actions->size() << endl;
00285         sb << "Num Observations : " << observations->size() << endl;
00286         sb << "X Trans : " << XTrans->ToString() << endl;
00287         sb << "XY Trans : " << XYTrans->ToString() << endl;
00288         sb << "Obs Prob : " << obsProb->ToString() << endl;
00289         sb << "Rewards : " << rewards->ToString() << endl;
00290 
00291         return sb.str();
00292 }
00293 
00294 void CoLoc::getObsProbVectorFast(obs_prob_vector& result, int a, int Xn, SparseVector& tmp1)
00295 {
00296         mult( result, tmp1, *obsProb->getMatrix(a, Xn) );
00297         // this should give the same result
00298         // mult( result, Otr[a][Xn], tmp1 );
00299 
00300         result *= (1.0/(result.norm_1()));
00301 
00302 }
00303 
00304 int  CoLoc::getNumActions()
00305 {
00306         return actions->size();
00307 }
00308 int  CoLoc::getBeliefSize()
00309 {
00310         return YStates->size();
00311 }
00312 
00313 SparseVector&  CoLoc::getJointUnobsStateProbVector(SparseVector& result, SharedPointer<BeliefWithState> b,      int a, int Xn) 
00314 {
00315         int Xc = b->sval; // currrent value for observed state variable
00316         // belief_vector Bc = b.bvec;   // current belief for unobserved state variable
00317         DenseVector tmp, tmp1;
00318         DenseVector Bc; // = b.bvec;
00319 
00320         copy(Bc, *(b->bvec));
00321 
00322         if (this->XStates->size() == 1)
00323         {
00324                 tmp = Bc;
00325         }
00326         else
00327         {
00328                 emult_column( tmp, *this->XTrans->getMatrix(a, Xc), Xn, Bc );
00329         }
00330 
00331         mult( tmp1, *this->XYTrans->getMatrixTr(a, Xc), tmp );
00332         
00333         copy(result, tmp1);
00334         return result;
00335 }
00336 
00337 void CoLoc::getObsProbVector(obs_prob_vector& result, const BeliefWithState& b, int a, int Xn) 
00338 {
00339         int Xc = b.sval; // currrent value for observed state variable
00340         // belief_vector Bc = b.bvec;   // current belief for unobserved state variable
00341         DenseVector tmp, tmp1, tmp2;
00342         DenseVector Bc; // = b.bvec;
00343 
00344         copy(Bc, *b.bvec);
00345 
00346         //cout << "a :" << a << " Xc :" << Xc << "Xn :" << Xn << endl;
00347         // --- overall: result = O_a_xn' * (TY_a_xc' * (TX_a_xc (:,xn) .* bc))
00348         // tmp = TX_a_xc (:,xn) .* bc
00349         emult_column( tmp, *XTrans->getMatrix(a, Xc), (int) Xn, Bc );
00350         // tmp1 = TY_a_xc' * tmp
00351         mult( tmp1, *XYTrans->getMatrixTr(a, Xc), tmp );
00352         // result = O_a_xn' * tmp1
00353         mult( tmp2, tmp1, *obsProb->getMatrix(a, Xn) );
00354 
00355         copy(result, tmp2);
00356         result *= (1.0/result.norm_1());
00357 }
00358 
00359 
00360 map<string, string> CoLoc::getActionsSymbols(int actionNum) 
00361 {
00362         map<string, string> result;
00363 
00364         return result;
00365 }
00366 
00367 
00368 map<string, string> CoLoc::getFactoredObservedStatesSymbols(int stateNum) 
00369 {
00370         map<string, string> result;
00371 
00372         return result;
00373 }
00374 
00375 
00376 map<string, string> CoLoc::getFactoredUnobservedStatesSymbols(int stateNum) 
00377 {
00378         map<string, string> result;
00379 
00380         return result;
00381 }
00382 
00383 map<string, string> CoLoc::getObservationsSymbols(int observationNum) 
00384 {
00385         map<string, string> result;
00386 
00387         return result;
00388 }
00389 


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