araplanner.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Maxim Likhachev
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the University of Pennsylvania nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 #ifndef __ARAPLANNER_H_
00030 #define __ARAPLANNER_H_
00031 
00032 
00033 //---configuration----
00034 
00035 //control of EPS
00036 //initial suboptimality bound (cost solution <= cost(eps*cost optimal solution)
00037 #define ARA_DEFAULT_INITIAL_EPS     5.0
00038 //as planning time exist, ARA* decreases epsilon bound
00039 #define ARA_DECREASE_EPS    0.2
00040 //final epsilon bound
00041 #define ARA_FINAL_EPS       1.0
00042 
00043 
00044 //---------------------
00045 
00046 #define ARA_INCONS_LIST_ID 0
00047 
00048 class CMDP;
00049 class CMDPSTATE;
00050 class CMDPACTION;
00051 class CHeap;
00052 class CList;
00053 
00054 
00055 //-------------------------------------------------------------
00056 
00057 
00060 typedef class ARASEARCHSTATEDATA : public AbstractSearchState
00061 {
00062 public:
00065         CMDPSTATE* MDPstate; 
00068         unsigned int v;
00071         unsigned int g;
00074         short unsigned int iterationclosed;
00077         short unsigned int callnumberaccessed;
00080         short unsigned int numofexpands;
00083         CMDPSTATE *bestpredstate;
00086         CMDPSTATE  *bestnextstate;
00087         unsigned int costtobestnextstate;
00088         int h;
00089 
00090         
00091 public:
00092         ARASEARCHSTATEDATA() {};        
00093         ~ARASEARCHSTATEDATA() {};
00094 } ARAState;
00095 
00096 
00097 
00100 typedef struct ARASEARCHSTATESPACE
00101 {
00102         double eps;
00103     double eps_satisfied;
00104         CHeap* heap;
00105         CList* inconslist;
00106         short unsigned int searchiteration;
00107         short unsigned int callnumber;
00108         CMDPSTATE* searchgoalstate;
00109         CMDPSTATE* searchstartstate;
00110         
00111         CMDP searchMDP;
00112 
00113         bool bReevaluatefvals;
00114     bool bReinitializeSearchStateSpace;
00115         bool bNewSearchIteration;
00116 
00117 } ARASearchStateSpace_t;
00118 
00119 
00120 
00123 class ARAPlanner : public SBPLPlanner
00124 {
00125 
00126 public:
00127 
00130         int replan(double allocated_time_secs, vector<int>* solution_stateIDs_V);
00133         int replan(double allocated_time_sec, vector<int>* solution_stateIDs_V, int* solcost);
00134 
00137     int set_goal(int goal_stateID);
00140     int set_start(int start_stateID);
00141 
00144     void costs_changed(StateChangeQuery const & stateChange);
00145 
00149     void costs_changed();
00150 
00151 
00154          int force_planning_from_scratch(); 
00155 
00158         int set_search_mode(bool bSearchUntilFirstSolution);
00159 
00162         virtual double get_solution_eps() const {return pSearchStateSpace_->eps_satisfied;};
00163 
00166     virtual int get_n_expands() const { return searchexpands; }
00167 
00170         virtual void set_initialsolution_eps(double initialsolution_eps) {finitial_eps = initialsolution_eps;};
00171 
00174         void print_searchpath(FILE* fOut);
00175 
00176 
00179     ARAPlanner(DiscreteSpaceInformation* environment, bool bforwardsearch);
00182     ~ARAPlanner();
00183 
00186   double get_initial_eps(){return finitial_eps;};
00187 
00190   double get_initial_eps_planning_time(){return finitial_eps_planning_time;}
00191 
00194   double get_final_eps_planning_time(){return final_eps_planning_time;};
00195 
00198   int get_n_expands_init_solution(){return num_of_expands_initial_solution;};
00199 
00202   double get_final_epsilon(){return final_eps;};
00203 
00204 private:
00205 
00206         //member variables
00207   double finitial_eps, finitial_eps_planning_time, final_eps_planning_time, final_eps;
00208 
00209   int num_of_expands_initial_solution;
00210 
00211         MDPConfig* MDPCfg_;
00212 
00213         bool bforwardsearch; //if true, then search proceeds forward, otherwise backward
00214 
00215         bool bsearchuntilfirstsolution; //if true, then search until first solution only (see planner.h for search modes)
00216 
00217     ARASearchStateSpace_t* pSearchStateSpace_;
00218 
00219         unsigned int searchexpands;
00220         int MaxMemoryCounter;
00221         clock_t TimeStarted;
00222         FILE *fDeb;
00223 
00224 
00225         //member functions
00226         void Initialize_searchinfo(CMDPSTATE* state, ARASearchStateSpace_t* pSearchStateSpace);
00227 
00228         CMDPSTATE* CreateState(int stateID, ARASearchStateSpace_t* pSearchStateSpace);
00229 
00230         CMDPSTATE* GetState(int stateID, ARASearchStateSpace_t* pSearchStateSpace);
00231 
00232         int ComputeHeuristic(CMDPSTATE* MDPstate, ARASearchStateSpace_t* pSearchStateSpace);
00233 
00234         //initialization of a state
00235         void InitializeSearchStateInfo(ARAState* state, ARASearchStateSpace_t* pSearchStateSpace);
00236 
00237         //re-initialization of a state
00238         void ReInitializeSearchStateInfo(ARAState* state, ARASearchStateSpace_t* pSearchStateSpace);
00239 
00240         void DeleteSearchStateData(ARAState* state);
00241 
00242         //used for backward search
00243         void UpdatePreds(ARAState* state, ARASearchStateSpace_t* pSearchStateSpace);
00244 
00245 
00246         //used for forward search
00247         void UpdateSuccs(ARAState* state, ARASearchStateSpace_t* pSearchStateSpace);
00248 
00249         int GetGVal(int StateID, ARASearchStateSpace_t* pSearchStateSpace);
00250 
00251         //returns 1 if the solution is found, 0 if the solution does not exist and 2 if it ran out of time
00252         int ImprovePath(ARASearchStateSpace_t* pSearchStateSpace, double MaxNumofSecs);
00253 
00254         void BuildNewOPENList(ARASearchStateSpace_t* pSearchStateSpace);
00255 
00256         void Reevaluatefvals(ARASearchStateSpace_t* pSearchStateSpace);
00257 
00258         //creates (allocates memory) search state space
00259         //does not initialize search statespace
00260         int CreateSearchStateSpace(ARASearchStateSpace_t* pSearchStateSpace);
00261 
00262         //deallocates memory used by SearchStateSpace
00263         void DeleteSearchStateSpace(ARASearchStateSpace_t* pSearchStateSpace);
00264 
00265         //debugging 
00266         void PrintSearchState(ARAState* state, FILE* fOut);
00267 
00268 
00269         //reset properly search state space
00270         //needs to be done before deleting states
00271         int ResetSearchStateSpace(ARASearchStateSpace_t* pSearchStateSpace);
00272 
00273         //initialization before each search
00274         void ReInitializeSearchStateSpace(ARASearchStateSpace_t* pSearchStateSpace);
00275 
00276         //very first initialization
00277         int InitializeSearchStateSpace(ARASearchStateSpace_t* pSearchStateSpace);
00278 
00279         int SetSearchGoalState(int SearchGoalStateID, ARASearchStateSpace_t* pSearchStateSpace);
00280 
00281 
00282         int SetSearchStartState(int SearchStartStateID, ARASearchStateSpace_t* pSearchStateSpace);
00283 
00284         //reconstruct path functions are only relevant for forward search
00285         int ReconstructPath(ARASearchStateSpace_t* pSearchStateSpace);
00286 
00287 
00288         void PrintSearchPath(ARASearchStateSpace_t* pSearchStateSpace, FILE* fOut);
00289 
00290         int getHeurValue(ARASearchStateSpace_t* pSearchStateSpace, int StateID);
00291 
00292         //get path 
00293         vector<int> GetSearchPath(ARASearchStateSpace_t* pSearchStateSpace, int& solcost);
00294 
00295 
00296         bool Search(ARASearchStateSpace_t* pSearchStateSpace, vector<int>& pathIds, int & PathCost, bool bFirstSolution, bool bOptimalSolution, double MaxNumofSecs);
00297 
00298 
00299 };
00300 
00301 
00302 #endif
00303 
00304 
00305 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


sbpl
Author(s): Maxim Likhachev/maximl@seas.upenn.edu
autogenerated on Fri Jan 18 2013 13:41:53