environment_precomputed_adjacency_list.h

Go to the documentation of this file.

#ifndef __PRECOMPUTED_ADJACENCY_LIST_H_
#define __PRECOMPUTED_ADJACENCY_LIST_H_

#include <iostream>
#include <vector>
#include <list>
#include <map>
#include <utility>
#include <assert.h>
#include <stdlib.h>

//using namespace std; 

// Necessary because some of the below includes assume it
#include "../../sbpl/headers.h"

struct Adjacency
{
  int neighbor;
  int cost;
};
typedef list<Adjacency> Adjacencies;
typedef Adjacencies::iterator AdjListIterator;

template <class Coords>
class AdjacencyListSBPLEnv : public DiscreteSpaceInformation
{

public:

  AdjacencyListSBPLEnv ();
  void writeToStream (ostream& str = cout);

  void addPoint (const Coords& c);

  bool hasPoint (const Coords& c);

  void removeLastPoints (unsigned int n=1);

  void setCost (const Coords& c1, const Coords& c2, int cost);
  void setCost (const Coords& c1, const Coords& c2);
  
  void setStartState (const Coords& c);
  void setGoalState (const Coords& c);

  vector<Coords> findOptimalPath (int* solution_cost);

  // Inherited DiscreteSpaceInformation ops
  bool InitializeEnv (const char* sEnvFile);
  bool InitializeMDPCfg (MDPConfig *MDPCfg);
  int  GetFromToHeuristic(int FromStateID, int ToStateID);
  int  GetGoalHeuristic(int stateID);
  int  GetStartHeuristic(int stateID);
  void SetAllActionsandAllOutcomes(CMDPSTATE* state);
  void SetAllPreds(CMDPSTATE* state);
  void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV);
  void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV);
  int  SizeofCreatedEnv();
  void PrintState(int stateID, bool bVerbose, FILE* fOut=NULL);
  void PrintEnv_Config(FILE* fOut);

private:

  void resetStateId2IndexMapping(void);

  // Members
  vector<Coords> points_;
  map<Coords,int> pointIds_;
  vector<Adjacencies> adjacency_vector_;
  int startStateId_;
  int goalStateId_;
};






template <class Coords>
AdjacencyListSBPLEnv<Coords>::AdjacencyListSBPLEnv () : startStateId_(-1), goalStateId_(-1)
{}


template <class Coords>
void AdjacencyListSBPLEnv<Coords>::writeToStream (ostream& str) 
{
  int numStates = points_.size();
  str << "Adjacency list SBPL Env " << endl;
  for (unsigned int i=0; i<points_.size(); i++) {
    str << i << ". " << points_[i] << ".  Neighbors: ";
    for (AdjListIterator iter=adjacency_vector_[i].begin(); iter!=adjacency_vector_[i].end(); iter++) {
      if (iter->neighbor < numStates) {
        str << "[" << points_[iter->neighbor] << " " << iter->cost << "] ";
      }
      else {
        str << "[To-Be-Deleted Edge] ";
      }
    }
    str << endl;
  }
}


template <class Coords>
void AdjacencyListSBPLEnv<Coords>::addPoint (const Coords& c)
{
  pointIds_[c] = points_.size();
  Adjacencies a;
  points_.push_back(c);
  adjacency_vector_.push_back(a);

  int* entry = new int[NUMOFINDICES_STATEID2IND];
  for (unsigned int i=0; i<NUMOFINDICES_STATEID2IND; i++) {
    entry[i]=-1;
  }
  StateID2IndexMapping.push_back(entry);
}

template <class Coords>
bool AdjacencyListSBPLEnv<Coords>::hasPoint (const Coords& c)
{
  return !(pointIds_.find(c) == pointIds_.end());
}


template <class Coords>
void AdjacencyListSBPLEnv<Coords>::removeLastPoints (unsigned int n)
{
  assert (n<=points_.size());
  for (unsigned int i=0; i<n; i++) {
    int num_points=points_.size();
    Adjacencies& a=adjacency_vector_.back();

    // Iterate over neighbors of this currently-being-deleted point
    for (Adjacencies::iterator adj_iter=a.begin(); adj_iter!=a.end(); adj_iter++) {
      assert(adj_iter->neighbor<num_points);
      Adjacencies& neighbor_adjacency_list=adjacency_vector_[adj_iter->neighbor];

      // Iterate over neighbors of the neighbor
      for (Adjacencies::iterator neighbor_adj_iter=neighbor_adjacency_list.begin(); neighbor_adj_iter!=neighbor_adjacency_list.end(); neighbor_adj_iter++) {

        // When we find the entry for the current point, remove it and exit inner loop
        if (neighbor_adj_iter->neighbor==num_points-1) {
          neighbor_adjacency_list.erase(neighbor_adj_iter);
          break;
        }
      }
    }
    adjacency_vector_.pop_back();
    pointIds_.erase(points_.back());
    points_.pop_back();
  }
}


template <class Coords>
void AdjacencyListSBPLEnv<Coords>::setCost (const Coords& c1, const Coords& c2)
{
  setCost (c1, c2, c1.heuristicDistanceTo(c2));
}

template <class Coords>
void AdjacencyListSBPLEnv<Coords>::setCost (const Coords& c1, const Coords& c2, int cost)
{
  // Figure out indices of the given points
  typename map<Coords,int>::iterator i1 = pointIds_.find(c1);
  typename map<Coords,int>::iterator i2 = pointIds_.find(c2);
  int index1=i1->second;
  int index2=i2->second;

  // Loop over which direction edge to add
  for (unsigned int j=0; j<2; j++) {
    int ind1, ind2;
    if (j==0) {
      ind1 = index1;
      ind2 = index2;
    }
    else {
      ind1 = index2;
      ind2 = index1;
    }

    // Now set cost of edge from ind1 to ind2
    Adjacencies& adj = adjacency_vector_[ind1];

    AdjListIterator i, last=adj.end();
    for (i=adj.begin(); (i!=last) && (i->neighbor!=ind2); i++);

    // If edge does not exist add it, else just set the cost of the existing edge
    if (i==last) {
      Adjacency a;
      a.neighbor=ind2;
      a.cost=cost;
      adj.insert(i, a);
    }
    else {
      i->cost=cost;
    }
  }
}


template <class Coords>
void AdjacencyListSBPLEnv<Coords>::setStartState (const Coords& c)
{
  typename map<Coords,int>::iterator i = pointIds_.find(c);
  assert(i != pointIds_.end());
  startStateId_ = i->second;
}

template <class Coords>
void AdjacencyListSBPLEnv<Coords>::setGoalState (const Coords& c)
{
  typename map<Coords,int>::iterator i = pointIds_.find(c);
  assert(i != pointIds_.end());
  goalStateId_ = i->second;
}

template <class Coords>
bool AdjacencyListSBPLEnv<Coords>::InitializeMDPCfg (MDPConfig *MDPCfg)
{
  MDPCfg->goalstateid = goalStateId_;
  MDPCfg->startstateid = startStateId_;
  return true;
}


template <class Coords>
int AdjacencyListSBPLEnv<Coords>::GetFromToHeuristic(int FromStateID, int ToStateID)
{
  return points_[FromStateID].heuristicDistanceTo(points_[ToStateID]);
}

template <class Coords>
int AdjacencyListSBPLEnv<Coords>::GetGoalHeuristic(int stateID)
{
  return GetFromToHeuristic (stateID, goalStateId_);
}

template <class Coords>
int AdjacencyListSBPLEnv<Coords>::GetStartHeuristic(int stateID)
{
  return GetFromToHeuristic (startStateId_, stateID);
}

template <class Coords>
void AdjacencyListSBPLEnv<Coords>::PrintState(int stateID, bool bVerbose, FILE* fOut)
{
  // Note we're ignoring the fOut argument
  cout << points_[stateID] << endl;
}

template <class Coords>
void AdjacencyListSBPLEnv<Coords>::PrintEnv_Config(FILE* fOut)
{
  // Note we're ignoring the fOut argument
  cout << "Adjacency list env" << endl;
}


template <class Coords>
int AdjacencyListSBPLEnv<Coords>::SizeofCreatedEnv()
{
  return points_.size();
}


template <class Coords>
bool AdjacencyListSBPLEnv<Coords>::InitializeEnv (const char* sEnvFile)
{
  // Nothing to do in initialization
  return true;
}



template <class Coords>
void AdjacencyListSBPLEnv<Coords>::SetAllActionsandAllOutcomes(CMDPSTATE* state)
{
  // goal state is absorbing
  if (state->StateID == goalStateId_) {
    return;
  }

  Adjacencies& v = adjacency_vector_[state->StateID];
  int actionIndex=0;

  for (AdjListIterator i=v.begin(); i!=v.end(); i++) {
    CMDPACTION* action = state->AddAction(actionIndex);
    action->AddOutcome(i->neighbor, i->cost, 1.0);
  }
}


  

template <class Coords>
void AdjacencyListSBPLEnv<Coords>::SetAllPreds(CMDPSTATE* state)
{
  // Apparently this is not always necessary
  cout << "Error: SetAllPreds not implemented for adjacency list";
  throw new SBPL_Exception();
}

template <class Coords>
void AdjacencyListSBPLEnv<Coords>::GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV)
{
  SuccIDV->clear();
  CostV->clear();

  if (SourceStateID == goalStateId_) {
    return;
  }

  Adjacencies& v = adjacency_vector_[SourceStateID];
  for (AdjListIterator i=v.begin(); i!=v.end(); i++) {
    SuccIDV->push_back(i->neighbor);
    CostV->push_back(i->cost);
  }
}

template <class Coords>
void AdjacencyListSBPLEnv<Coords>::GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV)
{
  cout << "Error: GetPreds not currently implemented for adjacency list";
  throw new SBPL_Exception();
}

template <class Coords>
vector<Coords> AdjacencyListSBPLEnv<Coords>::findOptimalPath (int* solution_cost)
{
  // Initialize ARA planner
  ARAPlanner p(this, true);
  p.set_start(startStateId_);
  p.set_goal(goalStateId_);
  p.set_initialsolution_eps(1.0);
  vector<int> solution;
  p.replan(1.0, &solution, solution_cost);

  vector<Coords> solutionPoints;
  for (unsigned int i=0; i<solution.size(); i++) {
    solutionPoints.push_back(points_[solution[i]]);
  }

  resetStateId2IndexMapping();

  return solutionPoints;
}

// There's some side effect where you have to reset this every time you call the ARA planner
template <class Coords>
void AdjacencyListSBPLEnv<Coords>::resetStateId2IndexMapping (void)
{
  for (unsigned int i=0; i<StateID2IndexMapping.size(); i++) {
    for (unsigned int j=0; j<NUMOFINDICES_STATEID2IND; j++) {
      StateID2IndexMapping[i][j]=-1;
    }
  }
}



#endif

// Local variables:
// mode:c++
// End: