FootstepPlannerEnvironment.h
Go to the documentation of this file.
00001 /*
00002  * A footstep planner for humanoid robots
00003  *
00004  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00005  * http://www.ros.org/wiki/footstep_planner
00006  *
00007  *
00008  * This program is free software: you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation, version 3.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00021 #ifndef FOOTSTEP_PLANNER_FOOTSTEPPLANNERENVIRONMENT_H_
00022 #define FOOTSTEP_PLANNER_FOOTSTEPPLANNERENVIRONMENT_H_
00023 
00024 #include <footstep_planner/helper.h>
00025 #include <footstep_planner/PathCostHeuristic.h>
00026 #include <footstep_planner/Heuristic.h>
00027 #include <footstep_planner/Footstep.h>
00028 #include <footstep_planner/PlanningState.h>
00029 #include <footstep_planner/State.h>
00030 #include <humanoid_nav_msgs/ClipFootstep.h>
00031 #include <sbpl/headers.h>
00032 
00033 #include <math.h>
00034 #include <vector>
00035 #include <tr1/unordered_set>
00036 #include <tr1/hashtable.h>
00037 
00038 
00039 namespace footstep_planner
00040 {
00041 struct environment_params
00042 {
00043   std::vector<Footstep> footstep_set;
00044   boost::shared_ptr<Heuristic> heuristic;
00045 
00047   std::vector<std::pair<int, int> > step_range;
00048 
00049   double footsize_x, footsize_y, footsize_z;
00050   double foot_origin_shift_x, foot_origin_shift_y;
00051   double max_footstep_x, max_footstep_y, max_footstep_theta;
00052   double max_inverse_footstep_x, max_inverse_footstep_y,
00053          max_inverse_footstep_theta;
00054   double step_cost;
00055   int    collision_check_accuracy;
00056   int    hash_table_size;
00057   double cell_size;
00058   int    num_angle_bins;
00059   bool   forward_search;
00060   double max_step_width;
00061   int    num_random_nodes;
00062   double random_node_distance;
00063   double heuristic_scale;
00064 };
00065 
00066 
00074 class FootstepPlannerEnvironment : public DiscreteSpaceInformation
00075 {
00076 public:
00077   // specialization of hash<int,int>, similar to standard boost::hash on pairs?
00078   struct IntPairHash{
00079   public:
00080     size_t operator()(std::pair<int, int> x) const throw() {
00081       size_t seed = std::tr1::hash<int>()(x.first);
00082       return std::tr1::hash<int>()(x.second) + 0x9e3779b9 + (seed<<6) + (seed>>2);
00083     }
00084   };
00085 
00086   typedef std::vector<int> exp_states_t;
00087   typedef exp_states_t::const_iterator exp_states_iter_t;
00088   typedef std::tr1::unordered_set<std::pair<int,int>, IntPairHash > exp_states_2d_t;
00089   typedef exp_states_2d_t::const_iterator exp_states_2d_iter_t;
00090 
00125   FootstepPlannerEnvironment(const environment_params& params);
00126 
00127   virtual ~FootstepPlannerEnvironment();
00128 
00134   std::pair<int, int> updateGoal(const State& foot_left,
00135                                  const State& foot_right);
00136 
00142   std::pair<int, int> updateStart(const State& foot_left,
00143                                   const State& right_right);
00144 
00145   void updateMap(gridmap_2d::GridMap2DPtr map);
00146 
00151   bool occupied(const State& s);
00152 
00158   bool getState(unsigned int id, State* s);
00159 
00164   void reset();
00165 
00167   int getNumExpandedStates() { return ivNumExpandedStates; };
00168 
00169   exp_states_2d_iter_t getExpandedStatesStart()
00170   {
00171     return ivExpandedStates.begin();
00172   };
00173 
00174   exp_states_2d_iter_t getExpandedStatesEnd()
00175   {
00176     return ivExpandedStates.end();
00177   };
00178 
00179   exp_states_iter_t getRandomStatesStart()
00180   {
00181     return ivRandomStates.begin();
00182   };
00183 
00184   exp_states_iter_t getRandomStatesEnd()
00185   {
00186     return ivRandomStates.end();
00187   };
00188 
00193   int GetFromToHeuristic(int FromStateID, int ToStateID);
00194 
00199   int GetGoalHeuristic(int stateID);
00200 
00205   int GetStartHeuristic(int stateID);
00206 
00212   void GetSuccs(int SourceStateID, std::vector<int> *SuccIDV,
00213                 std::vector<int> *CostV);
00214 
00220   void GetPreds(int TargetStateID, std::vector<int> *PredIDV,
00221                 std::vector<int> *CostV);
00222 
00228   virtual void GetRandomSuccsatDistance(int SourceStateID,
00229                                         std::vector<int>* SuccIDV,
00230                                         std::vector<int>* CLowV);
00231 
00237   virtual void GetRandomPredsatDistance(int TargetStateID,
00238                                         std::vector<int>* PredIDV,
00239                                         std::vector<int>* CLowV);
00240 
00242   void GetSuccsTo(int SourceStateID, int goalStateID,
00243                   std::vector<int> *SuccIDV, std::vector<int> *CostV);
00244 
00246   bool AreEquivalent(int StateID1, int StateID2);
00247 
00248   bool InitializeEnv(const char *sEnvFile);
00249 
00250   bool InitializeMDPCfg(MDPConfig *MDPCfg);
00251 
00252   void PrintEnv_Config(FILE *fOut);
00253 
00254   void PrintState(int stateID, bool bVerbose, FILE *fOut);
00255 
00256   void SetAllActionsandAllOutcomes(CMDPSTATE *state);
00257 
00258   void SetAllPreds(CMDPSTATE *state);
00259 
00260   int SizeofCreatedEnv();
00261 
00268   bool reachable(const PlanningState& from, const PlanningState& to);
00269 
00270   void getPredsOfGridCells(const std::vector<State>& changed_states,
00271                            std::vector<int>* pred_ids);
00272 
00273   void getSuccsOfGridCells(const std::vector<State>& changed_states,
00274                            std::vector<int>* succ_ids);
00275 
00281   void updateHeuristicValues();
00282 
00284   static const int cvMmScale = 1000;
00285 
00286 protected:
00291   int GetFromToHeuristic(const PlanningState& from, const PlanningState& to);
00292 
00294   int  stepCost(const PlanningState& a, const PlanningState& b);
00295 
00299   bool occupied(const PlanningState& s);
00300 
00301   void GetRandomNeighs(const PlanningState* currentState,
00302                        std::vector<int>* NeighIDV,
00303                        std::vector<int>* CLowV,
00304                        int nNumofNeighs, int nDist_c, bool bSuccs);
00305 
00306   void setStateArea(const PlanningState& left, const PlanningState& right);
00307 
00309   const PlanningState* createNewHashEntry(const State& s);
00310 
00318   const PlanningState* createNewHashEntry(const PlanningState& s);
00319 
00321   const PlanningState* getHashEntry(const State& s);
00322 
00327   const PlanningState* getHashEntry(const PlanningState& s);
00328 
00329   const PlanningState* createHashEntryIfNotExists(const PlanningState& s);
00330 
00335   bool closeToGoal(const PlanningState& from);
00336 
00341   bool closeToStart(const PlanningState& from);
00342 
00344   struct less
00345   {
00346     bool operator ()(const PlanningState* a,
00347                      const PlanningState* b) const;
00348   };
00349 
00354   int ivIdPlanningGoal;
00355 
00357   int ivIdStartFootLeft;
00359   int ivIdStartFootRight;
00361   int ivIdGoalFootLeft;
00363   int ivIdGoalFootRight;
00364 
00365   std::vector<int> ivStateArea;
00366 
00371   std::vector<const PlanningState*> ivStateId2State;
00372 
00378   std::vector<const PlanningState*>* ivpStateHash2State;
00379 
00381   const std::vector<Footstep>& ivFootstepSet;
00382 
00384   const boost::shared_ptr<Heuristic> ivHeuristicConstPtr;
00385 
00387   const double ivFootsizeX;
00389   const double ivFootsizeY;
00390 
00392   const double ivOriginFootShiftX;
00394   const double ivOriginFootShiftY;
00395 
00397   const int ivMaxFootstepX;
00399   const int ivMaxFootstepY;
00401   int ivMaxFootstepTheta;
00402 
00404   const int ivMaxInvFootstepX;
00406   const int ivMaxInvFootstepY;
00408   int ivMaxInvFootstepTheta;
00409 
00414   const int ivStepCost;
00415 
00421   const int ivCollisionCheckAccuracy;
00422 
00427   const int ivHashTableSize;
00428 
00430   const double ivCellSize;
00432   const int ivNumAngleBins;
00433 
00435   const bool ivForwardSearch;
00436 
00437   double ivMaxStepWidth;
00438 
00440   const int ivNumRandomNodes;
00442   const int ivRandomNodeDist;
00443 
00448   double ivHeuristicScale;
00449 
00451   bool ivHeuristicExpired;
00452 
00454   boost::shared_ptr<gridmap_2d::GridMap2D> ivMapPtr;
00455 
00456   exp_states_2d_t ivExpandedStates;
00457   exp_states_t ivRandomStates;  
00458   size_t ivNumExpandedStates;
00459 
00460   bool* ivpStepRange;
00461 };
00462 }
00463 
00464 #endif  // FOOTSTEP_PLANNER_FOOTSTEPPLANNERENVIRONMENT_H_


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Oct 6 2014 00:41:41