adplanner.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 __ADPLANNER_H_
00030 #define __ADPLANNER_H_
00031 
00032 
00033 //---configuration----
00034 
00035 //control of EPS
00036 //initial suboptimality bound (cost solution <= cost(eps*cost optimal solution)
00037 #define AD_DEFAULT_INITIAL_EPS      10.0
00038 //as planning time exist, AD* decreases epsilon bound
00039 #define AD_DECREASE_EPS    0.2
00040 //final epsilon bound
00041 #define AD_FINAL_EPS        1.0
00042 
00043 
00044 //---------------------
00045 
00046 #define AD_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 ADSEARCHSTATEDATA : 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;
00084 
00087         CMDPSTATE  *bestnextstate;
00088         unsigned int costtobestnextstate;
00089         int h;
00090 
00091         
00092 public:
00093         ADSEARCHSTATEDATA() {}; 
00094         ~ADSEARCHSTATEDATA() {};
00095 } ADState;
00096 
00097 
00098 
00101 typedef struct ADSEARCHSTATESPACE
00102 {
00103         double eps;
00104     double eps_satisfied;
00105         CHeap* heap;
00106         CList* inconslist;
00107         short unsigned int searchiteration;
00108         short unsigned int callnumber;
00109         CMDPSTATE* searchgoalstate;
00110         CMDPSTATE* searchstartstate;
00111         
00112         CMDP searchMDP;
00113 
00114         bool bReevaluatefvals;
00115     bool bReinitializeSearchStateSpace;
00116         bool bRebuildOpenList;
00117 
00118 } ADSearchStateSpace_t;
00119 
00120 
00121 
00124 class ADPlanner : public SBPLPlanner
00125 {
00126 
00127 public:
00130         int replan(double allocated_time_secs, vector<int>* solution_stateIDs_V);
00133         int replan(double allocated_time_secs, vector<int>* solution_stateIDs_V, int* solcost);
00134 
00137     int set_goal(int goal_stateID);
00138 
00141     int set_start(int start_stateID);
00142 
00145     int force_planning_from_scratch(); 
00146         
00149         int set_search_mode(bool bSearchUntilFirstSolution);
00150 
00153         void costs_changed(StateChangeQuery const & stateChange);
00154 
00159         void update_succs_of_changededges(vector<int>* succsIDV);
00160 
00165         void update_preds_of_changededges(vector<int>* predsIDV);
00166 
00169         virtual double get_solution_eps() const {return pSearchStateSpace_->eps_satisfied;};
00170 
00173     virtual int get_n_expands() const { return searchexpands; }
00174 
00177   double get_initial_eps(){return finitial_eps;};
00178 
00181   double get_initial_eps_planning_time(){return finitial_eps_planning_time;}
00182 
00185   double get_final_eps_planning_time(){return final_eps_planning_time;};
00186 
00189   int get_n_expands_init_solution(){return num_of_expands_initial_solution;};
00190 
00193   double get_final_epsilon(){return final_eps;};
00194 
00197         virtual void set_initialsolution_eps(double initialsolution_eps) {finitial_eps = initialsolution_eps;};
00198 
00199 
00202     ADPlanner(DiscreteSpaceInformation* environment, bool bForwardSearch);
00205     ~ADPlanner();
00206 
00207 
00208 
00209 private:
00210 
00211         //member variables
00212         double finitial_eps, finitial_eps_planning_time, final_eps_planning_time, final_eps;
00213         int num_of_expands_initial_solution;
00214         MDPConfig* MDPCfg_;
00215 
00216         bool bforwardsearch;
00217         bool bsearchuntilfirstsolution; //if true, then search until first solution (see planner.h for search modes)
00218 
00219     ADSearchStateSpace_t* pSearchStateSpace_;
00220 
00221         unsigned int searchexpands;
00222         int MaxMemoryCounter;
00223         clock_t TimeStarted;
00224         FILE *fDeb;
00225 
00226 
00227         //member functions
00228         void Initialize_searchinfo(CMDPSTATE* state, ADSearchStateSpace_t* pSearchStateSpace);
00229 
00230         CMDPSTATE* CreateState(int stateID, ADSearchStateSpace_t* pSearchStateSpace);
00231 
00232         CMDPSTATE* GetState(int stateID, ADSearchStateSpace_t* pSearchStateSpace);
00233 
00234         int ComputeHeuristic(CMDPSTATE* MDPstate, ADSearchStateSpace_t* pSearchStateSpace);
00235 
00236         //initialization of a state
00237         void InitializeSearchStateInfo(ADState* state, ADSearchStateSpace_t* pSearchStateSpace);
00238 
00239         //re-initialization of a state
00240         void ReInitializeSearchStateInfo(ADState* state, ADSearchStateSpace_t* pSearchStateSpace);
00241 
00242         void DeleteSearchStateData(ADState* state);
00243 
00244 
00245         //used for backward search
00246         void UpdatePredsofOverconsState(ADState* state, ADSearchStateSpace_t* pSearchStateSpace);
00247         void UpdatePredsofUnderconsState(ADState* state, ADSearchStateSpace_t* pSearchStateSpace);
00248 
00249         //used for forward search
00250         void UpdateSuccsofOverconsState(ADState* state, ADSearchStateSpace_t* pSearchStateSpace);
00251         void UpdateSuccsofUnderconsState(ADState* state, ADSearchStateSpace_t* pSearchStateSpace);
00252         
00253         void UpdateSetMembership(ADState* state);
00254         void Recomputegval(ADState* state);
00255 
00256 
00257         int GetGVal(int StateID, ADSearchStateSpace_t* pSearchStateSpace);
00258 
00259         //returns 1 if the solution is found, 0 if the solution does not exist and 2 if it ran out of time
00260         int ComputePath(ADSearchStateSpace_t* pSearchStateSpace, double MaxNumofSecs);
00261 
00262         void BuildNewOPENList(ADSearchStateSpace_t* pSearchStateSpace);
00263 
00264         void Reevaluatefvals(ADSearchStateSpace_t* pSearchStateSpace);
00265 
00266         //creates (allocates memory) search state space
00267         //does not initialize search statespace
00268         int CreateSearchStateSpace(ADSearchStateSpace_t* pSearchStateSpace);
00269 
00270         //deallocates memory used by SearchStateSpace
00271         void DeleteSearchStateSpace(ADSearchStateSpace_t* pSearchStateSpace);
00272 
00273 
00274         //reset properly search state space
00275         //needs to be done before deleting states
00276         int ResetSearchStateSpace(ADSearchStateSpace_t* pSearchStateSpace);
00277 
00278         //initialization before each search
00279         void ReInitializeSearchStateSpace(ADSearchStateSpace_t* pSearchStateSpace);
00280 
00281         //very first initialization
00282         int InitializeSearchStateSpace(ADSearchStateSpace_t* pSearchStateSpace);
00283 
00284         int SetSearchGoalState(int SearchGoalStateID, ADSearchStateSpace_t* pSearchStateSpace);
00285 
00286 
00287         int SetSearchStartState(int SearchStartStateID, ADSearchStateSpace_t* pSearchStateSpace);
00288 
00289         //reconstruct path functions are only relevant for forward search
00290         int ReconstructPath(ADSearchStateSpace_t* pSearchStateSpace);
00291 
00292 
00293         void PrintSearchState(ADState* searchstateinfo, FILE* fOut);
00294         void PrintSearchPath(ADSearchStateSpace_t* pSearchStateSpace, FILE* fOut);
00295 
00296         int getHeurValue(ADSearchStateSpace_t* pSearchStateSpace, int StateID);
00297 
00298         //get path 
00299         vector<int> GetSearchPath(ADSearchStateSpace_t* pSearchStateSpace, int& solcost);
00300 
00301 
00302         bool Search(ADSearchStateSpace_t* pSearchStateSpace, vector<int>& pathIds, int & PathCost, bool bFirstSolution, bool bOptimalSolution, double MaxNumofSecs);
00303 
00304         CKey ComputeKey(ADState* state);
00305 
00306         void Update_SearchSuccs_of_ChangedEdges(vector<int> const * statesIDV);
00307 
00308 
00309 };
00310 
00311 
00316 class StateChangeQuery
00317 {
00318 public:
00319   virtual ~StateChangeQuery() {}
00320   virtual std::vector<int> const * getPredecessors() const = 0;
00321   virtual std::vector<int> const * getSuccessors() const = 0;
00322 };
00323 
00324 
00325 #endif
00326 
00327 
00328 
 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:52