sbpl: sbpl: environment_precomputed_adjacency

00001 #ifndef __PRECOMPUTED_ADJACENCY_LIST_H_
00002 #define __PRECOMPUTED_ADJACENCY_LIST_H_
00003 
00004 #include <iostream>
00005 #include <vector>
00006 #include <list>
00007 #include <map>
00008 #include <utility>
00009 #include <assert.h>
00010 #include <stdlib.h>
00011 
00012 //using namespace std; 
00013 
00014 // Necessary because some of the below includes assume it
00015 #include "../../sbpl/headers.h"
00016 
00017 struct Adjacency
00018 {
00019   int neighbor;
00020   int cost;
00021 };
00022 typedef list<Adjacency> Adjacencies;
00023 typedef Adjacencies::iterator AdjListIterator;
00024 
00032 template <class Coords>
00033 class AdjacencyListSBPLEnv : public DiscreteSpaceInformation
00034 {
00035 
00036 public:
00037 
00038   AdjacencyListSBPLEnv ();
00039   void writeToStream (ostream& str = cout);
00040 
00043   void addPoint (const Coords& c);
00044 
00047   bool hasPoint (const Coords& c);
00048 
00051   void removeLastPoints (unsigned int n=1);
00052 
00057   void setCost (const Coords& c1, const Coords& c2, int cost);
00058   void setCost (const Coords& c1, const Coords& c2);
00059   
00060   void setStartState (const Coords& c);
00061   void setGoalState (const Coords& c);
00062 
00067   vector<Coords> findOptimalPath (int* solution_cost);
00068 
00069   // Inherited DiscreteSpaceInformation ops
00070   bool InitializeEnv (const char* sEnvFile);
00071   bool InitializeMDPCfg (MDPConfig *MDPCfg);
00072   int  GetFromToHeuristic(int FromStateID, int ToStateID);
00073   int  GetGoalHeuristic(int stateID);
00074   int  GetStartHeuristic(int stateID);
00075   void SetAllActionsandAllOutcomes(CMDPSTATE* state);
00076   void SetAllPreds(CMDPSTATE* state);
00077   void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV);
00078   void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV);
00079   int  SizeofCreatedEnv();
00080   void PrintState(int stateID, bool bVerbose, FILE* fOut=NULL);
00081   void PrintEnv_Config(FILE* fOut);
00082 
00083 private:
00084 
00085   void resetStateId2IndexMapping(void);
00086 
00087   // Members
00088   vector<Coords> points_;
00089   map<Coords,int> pointIds_;
00090   vector<Adjacencies> adjacency_vector_;
00091   int startStateId_;
00092   int goalStateId_;
00093 };
00094 
00095 
00096 
00097 
00098 
00099 
00100 template <class Coords>
00101 AdjacencyListSBPLEnv<Coords>::AdjacencyListSBPLEnv () : startStateId_(-1), goalStateId_(-1)
00102 {}
00103 
00104 
00105 template <class Coords>
00106 void AdjacencyListSBPLEnv<Coords>::writeToStream (ostream& str) 
00107 {
00108   int numStates = points_.size();
00109   str << "Adjacency list SBPL Env " << endl;
00110   for (unsigned int i=0; i<points_.size(); i++) {
00111     str << i << ". " << points_[i] << ".  Neighbors: ";
00112     for (AdjListIterator iter=adjacency_vector_[i].begin(); iter!=adjacency_vector_[i].end(); iter++) {
00113       if (iter->neighbor < numStates) {
00114         str << "[" << points_[iter->neighbor] << " " << iter->cost << "] ";
00115       }
00116       else {
00117         str << "[To-Be-Deleted Edge] ";
00118       }
00119     }
00120     str << endl;
00121   }
00122 }
00123 
00124 
00125 template <class Coords>
00126 void AdjacencyListSBPLEnv<Coords>::addPoint (const Coords& c)
00127 {
00128   pointIds_[c] = points_.size();
00129   Adjacencies a;
00130   points_.push_back(c);
00131   adjacency_vector_.push_back(a);
00132 
00133   int* entry = new int[NUMOFINDICES_STATEID2IND];
00134   for (unsigned int i=0; i<NUMOFINDICES_STATEID2IND; i++) {
00135     entry[i]=-1;
00136   }
00137   StateID2IndexMapping.push_back(entry);
00138 }
00139 
00140 template <class Coords>
00141 bool AdjacencyListSBPLEnv<Coords>::hasPoint (const Coords& c)
00142 {
00143   return !(pointIds_.find(c) == pointIds_.end());
00144 }
00145 
00146 
00147 template <class Coords>
00148 void AdjacencyListSBPLEnv<Coords>::removeLastPoints (unsigned int n)
00149 {
00150   assert (n<=points_.size());
00151   for (unsigned int i=0; i<n; i++) {
00152     int num_points=points_.size();
00153     Adjacencies& a=adjacency_vector_.back();
00154 
00155     // Iterate over neighbors of this currently-being-deleted point
00156     for (Adjacencies::iterator adj_iter=a.begin(); adj_iter!=a.end(); adj_iter++) {
00157       assert(adj_iter->neighbor<num_points);
00158       Adjacencies& neighbor_adjacency_list=adjacency_vector_[adj_iter->neighbor];
00159 
00160       // Iterate over neighbors of the neighbor
00161       for (Adjacencies::iterator neighbor_adj_iter=neighbor_adjacency_list.begin(); neighbor_adj_iter!=neighbor_adjacency_list.end(); neighbor_adj_iter++) {
00162 
00163         // When we find the entry for the current point, remove it and exit inner loop
00164         if (neighbor_adj_iter->neighbor==num_points-1) {
00165           neighbor_adjacency_list.erase(neighbor_adj_iter);
00166           break;
00167         }
00168       }
00169     }
00170     adjacency_vector_.pop_back();
00171     pointIds_.erase(points_.back());
00172     points_.pop_back();
00173   }
00174 }
00175 
00176 
00177 template <class Coords>
00178 void AdjacencyListSBPLEnv<Coords>::setCost (const Coords& c1, const Coords& c2)
00179 {
00180   setCost (c1, c2, c1.heuristicDistanceTo(c2));
00181 }
00182 
00183 template <class Coords>
00184 void AdjacencyListSBPLEnv<Coords>::setCost (const Coords& c1, const Coords& c2, int cost)
00185 {
00186   // Figure out indices of the given points
00187   typename map<Coords,int>::iterator i1 = pointIds_.find(c1);
00188   typename map<Coords,int>::iterator i2 = pointIds_.find(c2);
00189   int index1=i1->second;
00190   int index2=i2->second;
00191 
00192   // Loop over which direction edge to add
00193   for (unsigned int j=0; j<2; j++) {
00194     int ind1, ind2;
00195     if (j==0) {
00196       ind1 = index1;
00197       ind2 = index2;
00198     }
00199     else {
00200       ind1 = index2;
00201       ind2 = index1;
00202     }
00203 
00204     // Now set cost of edge from ind1 to ind2
00205     Adjacencies& adj = adjacency_vector_[ind1];
00206 
00207     AdjListIterator i, last=adj.end();
00208     for (i=adj.begin(); (i!=last) && (i->neighbor!=ind2); i++);
00209 
00210     // If edge does not exist add it, else just set the cost of the existing edge
00211     if (i==last) {
00212       Adjacency a;
00213       a.neighbor=ind2;
00214       a.cost=cost;
00215       adj.insert(i, a);
00216     }
00217     else {
00218       i->cost=cost;
00219     }
00220   }
00221 }
00222 
00223 
00224 template <class Coords>
00225 void AdjacencyListSBPLEnv<Coords>::setStartState (const Coords& c)
00226 {
00227   typename map<Coords,int>::iterator i = pointIds_.find(c);
00228   assert(i != pointIds_.end());
00229   startStateId_ = i->second;
00230 }
00231 
00232 template <class Coords>
00233 void AdjacencyListSBPLEnv<Coords>::setGoalState (const Coords& c)
00234 {
00235   typename map<Coords,int>::iterator i = pointIds_.find(c);
00236   assert(i != pointIds_.end());
00237   goalStateId_ = i->second;
00238 }
00239 
00240 template <class Coords>
00241 bool AdjacencyListSBPLEnv<Coords>::InitializeMDPCfg (MDPConfig *MDPCfg)
00242 {
00243   MDPCfg->goalstateid = goalStateId_;
00244   MDPCfg->startstateid = startStateId_;
00245   return true;
00246 }
00247 
00248 
00249 template <class Coords>
00250 int AdjacencyListSBPLEnv<Coords>::GetFromToHeuristic(int FromStateID, int ToStateID)
00251 {
00252   return points_[FromStateID].heuristicDistanceTo(points_[ToStateID]);
00253 }
00254 
00255 template <class Coords>
00256 int AdjacencyListSBPLEnv<Coords>::GetGoalHeuristic(int stateID)
00257 {
00258   return GetFromToHeuristic (stateID, goalStateId_);
00259 }
00260 
00261 template <class Coords>
00262 int AdjacencyListSBPLEnv<Coords>::GetStartHeuristic(int stateID)
00263 {
00264   return GetFromToHeuristic (startStateId_, stateID);
00265 }
00266 
00267 template <class Coords>
00268 void AdjacencyListSBPLEnv<Coords>::PrintState(int stateID, bool bVerbose, FILE* fOut)
00269 {
00270   // Note we're ignoring the fOut argument
00271   cout << points_[stateID] << endl;
00272 }
00273 
00274 template <class Coords>
00275 void AdjacencyListSBPLEnv<Coords>::PrintEnv_Config(FILE* fOut)
00276 {
00277   // Note we're ignoring the fOut argument
00278   cout << "Adjacency list env" << endl;
00279 }
00280 
00281 
00282 template <class Coords>
00283 int AdjacencyListSBPLEnv<Coords>::SizeofCreatedEnv()
00284 {
00285   return points_.size();
00286 }
00287 
00288 
00289 template <class Coords>
00290 bool AdjacencyListSBPLEnv<Coords>::InitializeEnv (const char* sEnvFile)
00291 {
00292   // Nothing to do in initialization
00293   return true;
00294 }
00295 
00296 
00297 
00298 template <class Coords>
00299 void AdjacencyListSBPLEnv<Coords>::SetAllActionsandAllOutcomes(CMDPSTATE* state)
00300 {
00301   // goal state is absorbing
00302   if (state->StateID == goalStateId_) {
00303     return;
00304   }
00305 
00306   Adjacencies& v = adjacency_vector_[state->StateID];
00307   int actionIndex=0;
00308 
00309   for (AdjListIterator i=v.begin(); i!=v.end(); i++) {
00310     CMDPACTION* action = state->AddAction(actionIndex);
00311     action->AddOutcome(i->neighbor, i->cost, 1.0);
00312   }
00313 }
00314 
00315 
00316   
00317 
00318 template <class Coords>
00319 void AdjacencyListSBPLEnv<Coords>::SetAllPreds(CMDPSTATE* state)
00320 {
00321   // Apparently this is not always necessary
00322   cout << "Error: SetAllPreds not implemented for adjacency list";
00323   throw new SBPL_Exception();
00324 }
00325 
00326 template <class Coords>
00327 void AdjacencyListSBPLEnv<Coords>::GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV)
00328 {
00329   SuccIDV->clear();
00330   CostV->clear();
00331 
00332   if (SourceStateID == goalStateId_) {
00333     return;
00334   }
00335 
00336   Adjacencies& v = adjacency_vector_[SourceStateID];
00337   for (AdjListIterator i=v.begin(); i!=v.end(); i++) {
00338     SuccIDV->push_back(i->neighbor);
00339     CostV->push_back(i->cost);
00340   }
00341 }
00342 
00343 template <class Coords>
00344 void AdjacencyListSBPLEnv<Coords>::GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV)
00345 {
00346   cout << "Error: GetPreds not currently implemented for adjacency list";
00347   throw new SBPL_Exception();
00348 }
00349 
00350 template <class Coords>
00351 vector<Coords> AdjacencyListSBPLEnv<Coords>::findOptimalPath (int* solution_cost)
00352 {
00353   // Initialize ARA planner
00354   ARAPlanner p(this, true);
00355   p.set_start(startStateId_);
00356   p.set_goal(goalStateId_);
00357   p.set_initialsolution_eps(1.0);
00358   vector<int> solution;
00359   p.replan(1.0, &solution, solution_cost);
00360 
00361   vector<Coords> solutionPoints;
00362   for (unsigned int i=0; i<solution.size(); i++) {
00363     solutionPoints.push_back(points_[solution[i]]);
00364   }
00365 
00366   resetStateId2IndexMapping();
00367 
00368   return solutionPoints;
00369 }
00370 
00371 // There's some side effect where you have to reset this every time you call the ARA planner
00372 template <class Coords>
00373 void AdjacencyListSBPLEnv<Coords>::resetStateId2IndexMapping (void)
00374 {
00375   for (unsigned int i=0; i<StateID2IndexMapping.size(); i++) {
00376     for (unsigned int j=0; j<NUMOFINDICES_STATEID2IND; j++) {
00377       StateID2IndexMapping[i][j]=-1;
00378     }
00379   }
00380 }
00381 
00382 
00383 
00384 #endif
00385 
00386 // Local variables:
00387 // mode:c++
00388 // End:
environment_precomputed_adjacency_list.h
