Installation Instructions

If you know you want to use E-Graphs with PR2 full body planning (coupled arm and base), just go here (https://sbpl.pc.cs.cmu.edu/redmine/projects/fbp/wiki) and follow these instructions.

If you know you want to use E-Graphs with single arm planning or navigation (x,y,theta or just x,y), just go here (https://sbpl.pc.cs.cmu.edu/redmine/projects/egraph-environments/wiki) and follow these instructions.

Otherwise, you want to run E-Graphs on your own domain, then follow the instructions below.

  1. Install the dev branch of sbpl
    git clone https://github.com/sbpl/sbpl.git -b dev
    

    In the sbpl directory run this:
    mkdir build
    cd build
    cmake ..
    make
    sudo make install
    
  2. Get E-Graphs
    git clone https://mlphilli@sbpl.pc.cs.cmu.edu/redmine/people/mike/e-graphs.git -b v3
    
  3. Make sure the E-Graph repo is on your ROS_PACKAGE_PATH
  4. Build
    rosmake egraphs
    

Making E-Graphs run on a new domain

These instructions will show you how to write an E-Graph wrapper for your environment (E-Graphs needs certain functions implemented to work) and how to then initialize the E-Graph/planner/heuristic/environment.

Writing an E-Graph environment wrapper

If you already have an SBPL environment (https://github.com/sbpl/sbpl) the first thing you will need to do is augment it with the functions needed for E-Graphs.
Below is a skeleton to get you started.
The EGraphMarkerMaker (and the 3 corresponding functions at the bottom of the class) come from the egraph_vis package. These are not required however, filling them in allows easy visualization of your Experience Graph in rviz.

#include<egraphs/egraphable.h>
#include<egraphs/egraph_discretize.h>
#include<egraph_vis/egraph_visualizer.h> //only needed if you want to use the E-Graph visualizer

class MyEGraphEnv : public YOUR_SBPL_ENVIRONMENT , public EGraphable<YOUR_HEURISTIC_TYPE>, 
                               public EGraphDiscretize, public EGraphMarkerMaker{
  public:

    /////////////////////// EGraphable Functions ///////////////////////

    //This is a domain specific function (and is not always possible). This is especially important if the choice of
    //heuristic is a down-projection from the original state space (for instance the heuristic guides a subset of 
    //the state varibles, like 2D grid search heuristic from the goal for an x,y,heading navigation problem.
    //In these cases, the E-Graph heuristic will guide the search to down-projections of experience and 
    //therefore, the search will have no guidance when trying to align the remaining state variables (for example
    //in the 2D heuristic for x,y,heading navigation the heuristic will guide the robot to have the same xy
    //coordinate as some state in the prior experience but then will struggle to align the heading). 
    //
    //To combat this, the planner will detect these scenarios and call this function to generate a "snap motion". 
    //Snap motions try to directly connect two states (one off the experience graph to one on it). 
    //If it is possible for your environment to directly connect an arbitrary (though nearby) pair of states
    //and you have a heuristic which only considers a subset of the state variables, you should implement this.
    //
    //"from" and "to" are the coordinates of the two states
    //if successful, you should fill in "id" with the id number of the "to" state, fill in "cost" with the 
    //cost of the motion, and return true
    //if the motion failed, return false ("id" and "cost" are irrelevant then)
    virtual bool snap(const std::vector<double>& from, const std::vector<double>& to, int& id, int& cost){
      return false;
    };

    //This function takes a state id and the environment should fill in the coordinate (vector of doubles) for 
    //the corresponding state. We promise to never call getCoord on the goal state (because it is possible
    //that the goal is underspecified and you may not know what the goal state will be).
    //This function returns true if the id actually exists and you were able to provide a coordinate.
    //It returns false if you could not. This should NEVER happen. The E-Graph planner should only call getCoord
    //on IDs which you already provided to us from getStateID or GetLazySuccsWithUniqueIds. 
    //
    //**This function MUST be the inverse of getStateID**
    //Use TestEGraphable if you want to confirm this.
    virtual bool getCoord(int id, std::vector<double>& coord) = 0;

    //This function takes a state vector and returns a state ID for it. Often, the coordinate we give you will
    //be one you have seen before and you should just be able to look up its ID. However, sometimes we will 
    //give you a coordinate you have never seen before. This generally should only happen when loading an E-Graph
    //from file or from a demonstration where a path was not generated by the planner itself.
    //This function must ALWAYS succeed. If you don't have a state ID for this coordinate yet you have to make one. 
    //
    //**This function MUST be the inverse of getCoord**
    //Use TestEGraphable if you want to confirm this.
    virtual int getStateID(const std::vector<double>& coord) = 0;

    //This take a state ID and returns if its corresponding state meets the goal conditions
    virtual bool isGoal(int id) = 0;

    //This function is called if the user tells the planner that the environment has changed. In that case
    //the planner will validate all edges and vertices in the experience graph. This function provides two states
    //"coord" and "coord2" you must check if edges connecting them is still valid (return true if so). 
    //If the edge is still valid, you can tell the E-Graph that the cost of the edge has changed "change_cost" and 
    //then if you set that to true, you should fill in "cost". If you set "change_cost" to false, the E-Graph will 
    //continue to use the previous edge cost.
    //If your environment is static (you know that edge costs will never change) you can just set change_cost to false
    //and always return true.
    virtual bool isValidEdge(const std::vector<double>& coord, const std::vector<double>& coord2, 
                                         bool& change_cost, int& cost) = 0;

    //This function is called if the user tells the planner that the environment has changed. In that case
    //the planner will validate all edges and vertices in the experience graph. This function provides a state 
    //coordinate and you return whether the state is still valid. If your environment is static you can just 
    //return true.
    virtual bool isValidVertex(const std::vector<double>& coord) = 0;

    //This function converts a state coordinate to a "heuristic coordinate". This depends on what heuristic you are
    //using. For instance, for euclidean distance heuristic, the heuristic coordinate is exactly the same as the 
    //state coordinate and "HeuristicType" would be a vector of doubles (you can see that the whole EGraphable
    //class is templated on the heuristic type). In the case of a 2D grid search heuristic for an x,y,heading
    //domain, the HeuristicType would be a vector of ints and the continuous x,y from the state would be
    //converted to discrete indices into the 2D grid.
    virtual void projectToHeuristicSpace(const std::vector<double>& coord, HeuristicType& heur_coord) const = 0;

    //Refer to the comment on projectToHeuristicSpace
    //The only difference is that this function returns the coordinate specifically for the goal. 
    //The reason we can't use the other function for the goal is that the goal might not be fully
    //specified and therefore we can't assume we have a coordinate to give you for the goal state.
    virtual void projectGoalToHeuristicSpace(HeuristicType& heur_coord) const = 0;

    /////////////////////// EGraphDiscretize Functions ///////////////////////

    //This function takes a continuous state vector and discretizes it. 
    //The planner assumes that states that have the same discretized coordinate
    //are equivalent.
    //The implementation usually involves dividing the continuous elements by
    //the resolution for that dimension. 
    //This function MUST be an inverse of discToCont
    //Use TestEGraphable if you want to confirm this.
    virtual void contToDisc(const std::vector<double>& c, std::vector<int>& d) = 0;

    //This function takes a discrete state vector and makes it continuous.
    //The implementation usually involves multiplying the discrete elements by
    //the resolution for that dimension (the choice of adding a "half cell" on 
    //is up to you....this is the difference between have continuous states in
    //the center of cells or on the border).
    //This function MUST be an inverse of contToDisc
    //Use TestEGraphable if you want to confirm this.
    virtual void discToCont(const std::vector<int>& d, std::vector<double>& c) = 0;

    /////////////////////// EGraphMarkerMaker Functions ///////////////////////
    //only needed if you want to use the E-Graph visualizer

    //Given a state vector, return a MarkerArray that represents the state in a compact way.
    //Every state in the E-Graph will be visualized at the same time using this function so
    //the marker should be small (typically a down projection) like a sphere otherwise
    //you won't be able to see anything. You will be able to click on these compact markers
    //to display a more detailed one on demand.
    virtual visualization_msgs::MarkerArray stateToVisualizationMarker(std::vector<double> coord) = 0;

    //Given two state vectors, return a MarkerArray that represents the edge in a compact way.
    //Every edge in the E-Graph will be visualized at the same time using this function so
    //the marker should be small (typically a down projection) like a line, otherwise
    //you won't be able to see anything. 
    virtual visualization_msgs::MarkerArray edgeToVisualizationMarker(std::vector<double> coord, 
                                                                     std::vector<double> coord2) = 0;

    //Given a state vector, return a MarkerArray that represents the state in a very detailed way,
    //such as all the robot's meshes in the proper configuration. Don't worry about cluttering
    //the display. These markers show up on demand by clicking one of the compact markers
    //(stateToVisualizationMarker). 
    virtual visualization_msgs::MarkerArray stateToDetailedVisualizationMarker(std::vector<double> coord) = 0;

};

Testing your EGraphable class

There are various functions in the EGraphable and EGraphDiscretize classes that need to be inverses like getStateID-getCoord and contToDisc-discToCont. If they are not you can get very strange bugs. We provide a TestEGraphable class to help you make sure your functions are inverses. Here is a code snippet showing you how to use it.

#include<egraphs/test_egraphable.h>

int main(int argc, char** argv){
  //initialize your environment
  //if you followed the above suggestions then this inherits from both EGraphable and EGraphDiscretize
  MyEGraphEnv env;
  vector<double> min_limits; //fill in the min limits for each dimension
  vector<double> max_limits; //fill in the max limits for each dimension
  TestEGraphable<YOUR_HEURISTIC_TYPE> test_egraphable(env, env, min_limits, max_limits); //create the test class
  test_egraphable.runTests(100); //run 100 tests

  return 0;
}

Setting up a planner node to call E-Graphs

#include <egraphs/egraphManager.h>
#include <egraphs/egraph_planner.h>
#include<egraphs/egraph.h>
#include<egraphs/egraph_3d_grid_heuristic.h> //this depends on the heuristic you want to use
#include<egraph_vis/egraph_visualizer.h> //optional if you want visualizations

int main(int argc, char** argv){

  //initialize your environment
  //if you followed the above suggestions then this inherits from both EGraphable and EGraphDiscretize
  //if you want to use the E-Graph visualizer then this should also inherit from EGraphMarkerMaker
  MyEGraphEnv env;

  //create a new E-Graph
  EGraph egraph(&env, NUMBER_OF_DIMENSIONS, NUMBER_OF_CONSTANTS);

  //or you can load a saved E-Graph from file
  //EGraph egraph(&env, "path_to_egraph.eg");

  //create your E-Graph heuristic
  //we have several heuristics implemented for you to choose from
  //or you can write your own! (more about heuristics in the next section)
  EGraph3dGridHeuristic heur(env, SIZE_OF_GRID_X, SIZE_OF_GRID_Y, 
                                                   SIZE_OF_GRID_Z, COST_OF_EACH_GRID_CELL);

  //create the E-Graph manager and planner
  EGraphManager<vector<int> > egraph_mgr(&egraph, &env, &heur);
  LazyAEGPlanner<vector<int> > planner(&env, true, &egraph_mgr);

  //create the E-Graph visualizer (optional)
  EGraphVisualizer egraph_vis(&egraph, &env);

  //set the start and goal states
  int start_id = env.YOUR_SET_START_FUNCTION();
  planner.set_start(start_id);
  env.YOUR_SET_GOAL_FUNCTION();

  //set the planning parameters
  EGraphReplanParams params(TIME_TO_PLAN);
  params.initial_eps = EPSILON; //something around 2.0 is usually good (encourages shortcut usage)
  params.dec_eps = EPSILON_DECREMENT_STEP; //used for anytime planning
  params.final_eps = FINAL_EPSILON; //used for anytime planning (set to EPSILON to not run anytime)
  params.epsE = EPSILON_E; //this is usually something larger 5, 10, 20 (pulls search toward E-Graph)
  params.dec_epsE = EPSILON_E_DECREMENT_STEP; //used for anytime planning
  params.final_epsE = FINAL_EPSILON_E; //used for anytime planning (set to EPSILON_E to not run anytime)
  params.return_first_solution = false; //if true, this runs until an initial solution is found. if false it runs 
                                        //until time expires or the final values of epsilon and epsilon_e have 
                                        //been reached (anytime planning)
  params.use_egraph = true; //use the E-Graph or not
  params.feedback_path = true; //after a path is found, add it to the E-Graph

  //call the planner
  vector<int> solution_stateIDs; //the planner will fill in this path of state ids
  bool ret = planner.replan(&solution_stateIDs, params); //ret is true on success

  //get planning statistics
  map<string,double> stats = planner.getStats();

  //you can write your E-Graph to file if you want
  egraph.save("my_egraph.eg");

  //convert the state ID path into a coordinate path
  vector<vector<double> > path(solution_stateIDs.size(), vector<double>(NUMBER_OF_DIMENSIONS, 0));
  for(unsigned int i=0; i<solution_stateIDs.size(); i++)
    env.getCoord(solution_stateIDs[i], path[i]);

  //visualize the E-Graph
  egraph_vis.visualize();

  return 0;
}

Choosing an E-Graph heuristic

The E-Graph heuristic is at the core of Experience Graphs. Correct implementation is crucial to getting the search to be guided toward prior experience. The heuristic should also be implemented efficiently since it will be used many times during planning. We have provided 3 pre-made heuristics that you can use. The heuristics should cover many domains.
  • EGraph3dGridHeuristic - Useful if a good heuristic for your planner is to project to xyz space and run a Dijkstra search backward from the goal. The heuristic computation is efficient (the E-Graph is projected into xyz space). It works well for arm planning, where the heuristic is for a spherical free-floating end effector. It also works well as a heuristic for UAVs.
  • EGraph2dGridHeuristic - Useful if a good heuristic for your planner is to project to xy space and run a Dijkstra search backward from the goal. The heuristic computation is efficient (the E-Graph is projected into xy space). It works well for x,y,heading navigation, where the heuristic is for a circular robot inscribed in the actual robot's footprint.
  • EGraphEuclideanHeuristic - A common heuristic which is nice because no down-projection is done (it guides all state variables). The current implementation is not that efficient (this will change in the coming months!).

Writing your own E-Graph heuristic (ADVANCED)

Refer to egraph_heuristic.h for the API that must be implemented and some documentation on how each should be implemented. A more detailed tutorial is coming soon...