Go to the documentation of this file.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
00013
00014
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
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
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
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
00161 for (Adjacencies::iterator neighbor_adj_iter=neighbor_adjacency_list.begin(); neighbor_adj_iter!=neighbor_adjacency_list.end(); neighbor_adj_iter++) {
00162
00163
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
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
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
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
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
00271 cout << points_[stateID] << endl;
00272 }
00273
00274 template <class Coords>
00275 void AdjacencyListSBPLEnv<Coords>::PrintEnv_Config(FILE* fOut)
00276 {
00277
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
00293 return true;
00294 }
00295
00296
00297
00298 template <class Coords>
00299 void AdjacencyListSBPLEnv<Coords>::SetAllActionsandAllOutcomes(CMDPSTATE* state)
00300 {
00301
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
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
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
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
00387
00388