31 #include <sys/times.h>
33 #include <boost/numeric/ublas/matrix.hpp>
34 #include <boost/numeric/ublas/matrix_proxy.hpp>
35 #include <boost/numeric/ublas/io.hpp>
47 #define DEBUG_AlphaVectorPlanning_BeliefSampling 0
48 #define DEBUG_AlphaVectorPlanning_BackProject 0
49 #define DEBUG_AlphaVectorPlanning_BackProjectFullPrintout 0
50 #define DEBUG_AlphaVectorPlanning_BackProjectFullSanityCheck 0
51 #define DEBUG_AlphaVectorPlanning_Prune 0
52 #define DEBUG_AlphaVectorPlanning_CrossSum 0
53 #define DEBUG_AlphaVectorPlanning_ValueFunctionToQ 0
54 #define DEBUG_AlphaVectorPlanning_ImportValueFunction 0
56 #define AlphaVectorPlanning_CheckForDuplicates 1
57 #define AlphaVectorPlanning_UseUBLASinBackProject 1
58 #define AlphaVectorPlanning_VerifyUBLASinBackProject 0
59 #define AlphaVectorPlanning_UseFastSparseBackup 1
74 if((tms=dynamic_cast<const TransitionModelMappingSparse *>(td)))
76 else if((tm=dynamic_cast<const TransitionModelMapping *>(td)))
79 throw(
E(
"AlphaVectorPlanning::Ctor() TransitionModelDiscretePtr not handled"));
105 #if AlphaVectorPlanning_UseFastSparseBackup
117 if((*
_m_Ts[a])(s,s1)!=0)
118 (*temp)(s1)=(*
_m_Ts[a])(s,s1);
129 if((*
_m_Os[a])(s1,o)!=0)
130 (*temp)(s1)=(*
_m_Os[a])(s1,o);
208 int nrS=(v[0].GetValues()).size();
210 for(
int k=0;k!=nrInV;k++)
211 for(
int s=0;s!=nrS;s++)
212 v1(k,s)=v[k].GetValue(s);
249 throw(
E(
"AlphaVectorPlanning::BackProjectFull attempting to backproject empty value function"));
251 #if DEBUG_AlphaVectorPlanning_BackProject
253 clock_t ticks_before, ticks_after;
254 ticks_before = times(&timeStruct);
261 #if AlphaVectorPlanning_UseUBLASinBackProject
266 #if AlphaVectorPlanning_CheckForDuplicates
269 vector<int> duplicates(nrInV,-1);
273 using namespace boost::numeric::ublas;
275 for(
unsigned int a=0;a!=nrA;a++)
276 for(
unsigned int o=0;o!=nrO;o++)
278 #if AlphaVectorPlanning_UseUBLASinBackProject
279 matrix_column<const ObservationModelMapping::Matrix> mO(*
_m_O[a],o);
281 for(
unsigned int k=0;k!=nrInV;k++)
283 if(duplicates[k]==-1)
285 #if AlphaVectorPlanning_UseUBLASinBackProject
286 const matrix_row<VectorSet> mV(vv,k);
288 for(
unsigned int s=0;s!=nrS;s++)
290 #if AlphaVectorPlanning_UseUBLASinBackProject
291 matrix_row<const TransitionModelMapping::Matrix>
293 x=inner_prod(element_prod(mT,mO),mV);
294 #if AlphaVectorPlanning_VerifyUBLASinBackProject
296 for(
unsigned int s1=0;s1!=nrS;s1++)
297 x1+=(*
_m_O[a])(s1,o)*(*
_m_T[a])(s,s1)*v(k,s1);
300 cerr << x <<
" " << x1 <<
" " << x-x1 << endl;
304 #else // AlphaVectorPlanning_UseUBLASinBackProject
306 for(
unsigned int s1=0;s1!=nrS;s1++)
307 x+=(*
_m_O[a])(s1,o)*(*
_m_T[a])(s,s1)*v(k,s1);
315 for(
unsigned int s=0;s!=nrS;s++)
324 #if DEBUG_AlphaVectorPlanning_BackProjectFullPrintout
325 cout <<
"BackProjectFull of:" << endl;
326 for(
unsigned int k=0;k!=nrInV;k++)
328 for(
unsigned int s=0;s!=nrS;s++)
329 cout << v(k,s) <<
" ";
334 for(
unsigned int a=0;a!=nrA;a++)
335 for(
unsigned int o=0;o!=nrO;o++)
337 cout <<
"Gao a " << a <<
" o " << o << endl;
340 for(
unsigned int k=0;k!=VS->size1();k++)
342 for(
unsigned int s=0;s!=VS->size2();s++)
343 cout << (*VS)(k,s) <<
" ";
349 #if DEBUG_AlphaVectorPlanning_BackProjectFullSanityCheck
350 double maxInV=-DBL_MAX;
351 for(
unsigned int k=0;k!=nrInV;k++)
352 for(
unsigned int s=0;s!=nrS;s++)
353 maxInV=max(maxInV,v(k,s));
355 double maxInGao=-DBL_MAX;
356 for(
unsigned int a=0;a!=nrA;a++)
357 for(
unsigned int o=0;o!=nrO;o++)
358 for(
unsigned int k=0;k!=nrInV;k++)
359 for(
unsigned int s=0;s!=nrS;s++)
360 maxInGao=max(maxInGao,(*G[a][o])(k,s));
364 cout <<
"Max value in V is " << maxInV <<
", max in Gao is "
370 #if DEBUG_AlphaVectorPlanning_BackProject
371 ticks_after = times(&timeStruct);
372 cout <<
"AlphaVectorPlanning::BackProject done in "
373 << ticks_after - ticks_before <<
" clock ticks, "
374 <<
static_cast<double>((ticks_after - ticks_before))
375 / sysconf(_SC_CLK_TCK)
395 throw(
E(
"AlphaVectorPlanning::BackProjectSparse attempting to backproject empty value function"));
404 #if AlphaVectorPlanning_CheckForDuplicates
407 vector<int> duplicates(nrInV,-1);
411 using namespace boost::numeric::ublas;
413 for(
unsigned int a=0;a!=nrA;a++)
414 for(
unsigned int o=0;o!=nrO;o++)
416 #if !AlphaVectorPlanning_UseFastSparseBackup
417 matrix_column<const ObservationModelMappingSparse::SparseMatrix>
420 for(
unsigned int k=0;k!=nrInV;k++)
422 if(duplicates[k]==-1)
424 const matrix_row<VectorSet> mV(vv,k);
426 for(
unsigned int s=0;s!=nrS;s++)
428 #if AlphaVectorPlanning_UseFastSparseBackup
433 x=inner_prod(element_prod(mT,mO),mV);
441 for(
unsigned int s=0;s!=nrS;s++)
471 bool foundEqualBelief,equal,addBelief;
478 cout <<
"Warning: sampling beliefs for an infinite horizon "
479 <<
"without reset." << endl;
510 double valMax=-DBL_MAX;
512 for(
int aQMDP=0;aQMDP!=nrA;++aQMDP)
514 double qQMDP=qmdp->
GetQ(b0,aQMDP);
525 a=
static_cast<int>(nrA*(rand() / (RAND_MAX + 1.0)));
537 foundEqualBelief=
false;
539 for(
int j=0;j!=i;j++)
545 if(S[j]->Get(s)!=b1.
Get(s))
554 foundEqualBelief=
true;
572 throw(
E(
"AlphaVectorPlanning::BeliefSampling belief fails sanity check"));
579 cout <<
"AlphaVectorPlanning::SampleBeliefs sampled belief nr "
580 << i <<
"/" << args.
nrBeliefs <<
" (nrEqualFound "
581 << nrEqualFound <<
")" << endl;
599 cout <<
"AlphaVectorPlanning::SampleBeliefs: warning, only "
600 <<
"managed to sample " << i <<
" unique beliefs instead of "
603 for(
int j=0;j!=i;j++)
618 #if DEBUG_AlphaVectorPlanning_CrossSum
619 cout <<
"AlphaVectorPlanning::CrossSum of " << nrInA
620 <<
" times " << nrInB << endl;
626 for(
int i=0;i!=nrInA;i++)
627 for(
int j=0;j!=nrInB;j++)
630 for(
int s=0;s!=nrS;s++)
631 C(k,s)=A(i,s)+B(j,s);
640 bool doIncPrune)
const
646 #if DEBUG_AlphaVectorPlanning_CrossSum
647 cout <<
"AlphaVectorPlanning::MonahanCrossSum for action " << a << endl;
658 Ga.resize(Ga1.size1(),Ga1.size2());
664 Ga.resize(Ga1.size1(),Ga1.size2());
670 for(
unsigned int k=0;k!=Ga.size1();k++)
673 for(
int s=0;s!=nrS;s++)
708 for(
int k=0;k!=nrInV;k++)
709 for(
int s=0;s!=nrS;s++)
722 bool dominated,valuesDominated;
723 vector<bool> dominatedVectors(V.size(),
false);
726 #if DEBUG_AlphaVectorPlanning_Prune
727 cout <<
"AlphaVectorPlanning::Prune " << V.size() <<
" vectors" << endl;
730 vector<AlphaVector>::const_iterator it=V.begin();
731 vector<AlphaVector>::const_iterator it1;
738 while(!dominatedVectors[i] && it1!=V.end() && !dominated)
740 valuesDominated=
true;
741 for(
int s=0;s!=nrS;s++)
742 if(it->GetValue(s) > it1->GetValue(s))
743 valuesDominated=
false;
745 if(valuesDominated && it1->GetAction()==it->GetAction() && it1!=it)
755 #if DEBUG_AlphaVectorPlanning_Prune
756 cout <<
"AlphaVectorPlanning::Prune added vector " << i << endl;
760 dominatedVectors[i]=
true;
766 #if DEBUG_AlphaVectorPlanning_Prune
767 cout <<
"AlphaVectorPlanning::Prune reduced " << V.size() <<
" to "
768 << V1.size() << endl;
777 int nrInV=V.size1(),nrS=V.size2();
778 bool dominated,valuesDominated;
779 vector<bool> dominatedVectors(nrInV,
false);
781 vector<int> vectorsToKeep;
783 #if DEBUG_AlphaVectorPlanning_Prune
784 cout <<
"AlphaVectorPlanning::Prune " << nrInV <<
" vectors" << endl;
787 for(it=0;it!=nrInV;it++)
792 while(!dominatedVectors[it] && it1!=nrInV && !dominated)
794 valuesDominated=
true;
795 for(
int s=0;s!=nrS;s++)
796 if(V(it,s) > V(it1,s))
797 valuesDominated=
false;
799 if(valuesDominated && it1!=it)
808 vectorsToKeep.push_back(it);
809 #if DEBUG_AlphaVectorPlanning_Prune
810 cout <<
"AlphaVectorPlanning::Prune added vector " << it << endl;
814 dominatedVectors[it]=
true;
818 int newNrInV=vectorsToKeep.size();
820 for(
int i=0;i!=newNrInV;i++)
821 for(
int s=0;s!=nrS;s++)
822 V1(i,s)=V(vectorsToKeep[i],s);
824 #if DEBUG_AlphaVectorPlanning_Prune
825 cout <<
"AlphaVectorPlanning::Prune reduced " << nrInV <<
" to "
844 for(
Index t=0;t!=Q.size();++t)
846 stringstream filenameT;
847 filenameT << filename <<
"_t" << t;
854 size_t nrA,
size_t nrS)
857 for(
Index t=0;t!=nr;++t)
859 stringstream filenameT;
860 filenameT << filename <<
"_t" << t;
880 vector<double> values;
882 ofstream fp(filename.c_str());
885 cerr <<
"AlphaVectorPlanning::ExportValueFunction: failed to "
886 <<
"open file " << filename << endl;
889 for(
unsigned int i=0;i!=V.size();i++)
891 values=V[i].GetValues();
892 int nrS=values.size();
894 fp << V[i].GetAction() <<
" " << V[i].GetBetaI() << endl;
895 for(
int s=0;s!=nrS;s++)
910 const int bufsize=65536;
911 char buffer[bufsize];
921 vector<double> values;
922 vector<int> actionBetaI;
923 long long int actionOrBetaI;
925 ifstream fp(filename.c_str());
928 cerr <<
"AlphaVectorPlanning::ImportValueFunction: failed to "
929 <<
"open file " << filename << endl;
932 while(!fp.getline(buffer,bufsize).eof())
941 istringstream is(buffer);
942 while(is >> actionOrBetaI)
943 actionBetaI.push_back(actionOrBetaI);
946 switch(actionBetaI.size())
949 action=actionBetaI[0];
953 action=actionBetaI[0];
954 betaI=actionBetaI[1];
957 throw(
E(
"AlphaVectorPlanning::ImportValueFunction parse error"));
967 istringstream is(buffer);
969 values.push_back(value);
974 nrStates=values.size();
985 #if DEBUG_AlphaVectorPlanning_ImportValueFunction
986 cout <<
"AlphaVectorPlanning::ImportValueFunction "
987 <<
"added vector " << V.size() <<
" for action "
988 << action <<
" betaI " << betaI << endl;
1022 for(
Index a=0;a<nrA;a++)
1025 for(
Index s=0;s<nrS;s++)
1027 V0.push_back(alpha);
1038 for(
QFDcit i=Q.begin();i!=Q.end();++i)
1039 for(
VFPDcit j=i->begin();j!=i->end();++j)
1058 ofstream fp(filename.c_str());
1061 cerr <<
"AlphaVectorPOMDP::ExportPOMDPFile: failed to open file "
1062 << filename << endl;
1069 fp <<
"values: reward" << endl;
1072 fp <<
"values: cost" << endl;
1076 for(
int s=0;s<nrS;s++)
1081 for(
int a=0;a<nrA;a++)
1085 fp <<
"observations:";
1086 for(
int o=0;o<nrO;o++)
1094 for(
int s0=0;s0<nrS;s0++)
1096 double bs = isd->
Get(s0);
1104 for(
int a=0;a<nrA;a++)
1105 for(
int s0=0;s0<nrS;s0++)
1106 for(
int s1=0;s1<nrS;s1++)
1110 fp <<
"T: " << a <<
" : " << s0 <<
" : " << s1 <<
" "
1114 for(
int a=0;a<nrA;a++)
1115 for(
int o=0;o<nrO;o++)
1116 for(
int s1=0;s1<nrS;s1++)
1120 fp <<
"O: " << a <<
" : " << s1 <<
" : " << o <<
" "
1124 for(
int a=0;a<nrA;a++)
1125 for(
int s0=0;s0<nrS;s0++)
1129 fp <<
"R: " << a <<
" : " << s0 <<
" : * : * "
1136 const string &filename)
1138 ofstream fp(filename.c_str());
1141 cerr <<
"AlphaVectorPlanning::ExportBeliefSet: failed to open file "
1142 << filename << endl;
1146 for(
int b=0;b!=nrB;b++)
1148 for(
unsigned int s=0;s!=B[b]->Size();s++)
1149 fp << B[b]->Get(s) <<
" ";
1158 GetPU()->GetNrStates()));
1163 size_t nrA,
size_t nrS)
1167 for(
Index a=0;a!=nrA;a++)
1170 for(
Index i=0;i!=V.size();i++)
1172 if(V[i].GetAction()==a)
1181 Q.push_back(dominatedVector);
1182 #if DEBUG_AlphaVectorPlanning_ValueFunctionToQ
1183 cout <<
"AlphaVectorPlanning::GetQFunctionsFromV: action " << a
1184 <<
" is dominated" << endl;
1189 #if DEBUG_AlphaVectorPlanning_ValueFunctionToQ
1190 cout <<
"AlphaVectorPlanning::GetQFunctionsFromV: action " << a
1191 <<
" has " << Q.size()
1192 <<
" vector(s) " << endl;
1208 int nrInV=V.size1(), nrS=V.size2();
1211 vector<int> duplicates(nrInV,-1);
1213 for(
int i=1;i!=nrInV;++i)
1215 for(
int j=0;j!=i;++j)
1218 for(
int s=0;s!=nrS;++s)
1234 #if 0 // reduce verbosity
1246 for(
VFPDcit it=V.begin(); it!=V.end(); ++it)