LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
LEMUR.h
Go to the documentation of this file.
1 
7 namespace ompl_lemur
8 {
9 
18 class LEMUR : public ompl::base::Planner
19 {
20 public:
21  // part 1: typedefs
22 
23  struct VProps
24  {
25  // from roadmap
26  ompl::base::State * state;
27  int batch;
28  bool is_shadow;
29  // collision status (index or pointer?)
30  size_t tag;
31  };
32  struct EProps
33  {
34  size_t index;
35  // from roadmap
36  double distance;
37  int batch;
38  // for lazysp; for current subset only!
39  double w_lazy;
40  // interior points, in bisection order
41  // if edge_states.size() != num_edge_states,
42  // then edge_states needs to be generated! (with tags = 0)
43  size_t num_edge_states;
44  std::vector< ompl::base::State * > edge_states;
45  //std::vector< size_t > edge_tags;
46  size_t edge_tag; // this is the tag for ALL INTERNAL STATES
47  };
48  typedef boost::adjacency_list<
49  boost::vecS, // Edgelist ds, for per-vertex out-edges
50  boost::vecS, // VertexList ds, for vertex set
51  boost::undirectedS, // type of graph
52  VProps, // internal (bundled) vertex properties
53  EProps // internal (bundled) edge properties
54  > Graph;
55 
56  typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
57  typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
58  typedef boost::graph_traits<Graph>::edge_descriptor Edge;
59  typedef boost::graph_traits<Graph>::edge_iterator EdgeIter;
60  typedef boost::graph_traits<Graph>::out_edge_iterator OutEdgeIter;
61 
62  // types of property maps to bundled properties
63  // these are needed by roadmap
64  typedef boost::property_map<Graph, ompl::base::State * VProps::*>::type VPStateMap;
65  typedef boost::property_map<Graph, int VProps::*>::type VPBatchMap;
66  typedef boost::property_map<Graph, bool VProps::*>::type VPIsShadowMap;
67  typedef boost::property_map<Graph, size_t VProps::*>::type VPTagMap;
68  typedef boost::property_map<Graph, std::size_t EProps::*>::type EPIndexMap;
69  typedef boost::property_map<Graph, double EProps::*>::type EPDistanceMap;
70  typedef boost::property_map<Graph, int EProps::*>::type EPBatchMap;
71  typedef boost::property_map<Graph, double EProps::*>::type EPWlazyMap;
72  //typedef boost::property_map<Graph, std::vector<size_t> EProps::*>::type EPTagsMap;
73  typedef boost::property_map<Graph, size_t EProps::*>::type EPTagMap;
74 
75  // for indexing
77  typedef boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
78  typedef boost::property_map<Graph, std::size_t EProps::*>::type EdgeIndexMap;
79  typedef EdgeIndexedGraph::EdgeVectorMap EdgeVectorMap;
80 
81  // for tag cache object
84 
85  // roadmap generator type
86  //typedef ompl_lemur::NNOmplBatched<Graph,VPStateMap> NN;
87  //typedef ompl_lemur::NNLinear<Graph,VPStateMap> NN;
88 
89  //typedef ompl::NearestNeighbors<Vertex> NN; // option A
91 
93 
94  // roots overlay graph (used internally)
95  // create an overlay graph for roots
96  struct OverVProps
97  {
98  Vertex core_vertex;
99  // like VProps
100  ompl::base::State * state;
101  int batch;
102  bool is_shadow;
103  size_t tag;
104  };
105  struct OverEProps
106  {
107  Edge core_edge;
108  // like EProps
109  double distance;
110  int batch;
111  double w_lazy;
112  bool is_evaled;
113  size_t num_edge_states;
114  std::vector< ompl::base::State * > edge_states;
115  //std::vector< size_t > edge_tags;
116  size_t edge_tag; // mega tag?
117  };
118  typedef boost::adjacency_list<
119  boost::vecS, // Edgelist ds, for per-vertex out-edges
120  boost::listS, // VertexList ds, for vertex set
121  boost::undirectedS, // type of graph
122  OverVProps, // internal (bundled) vertex properties
123  OverEProps // internal (bundled) edge properties
124  > OverGraph;
125  typedef boost::graph_traits<OverGraph>::vertex_descriptor OverVertex;
126  typedef boost::graph_traits<OverGraph>::vertex_iterator OverVertexIter;
127  typedef boost::graph_traits<OverGraph>::edge_descriptor OverEdge;
128  typedef boost::graph_traits<OverGraph>::edge_iterator OverEdgeIter;
129 
131  {
132  EPBatchMap edge_batch_map;
133  unsigned int num_batches;
134  filter_num_batches() {}
135  filter_num_batches(EPBatchMap edge_batch_map, unsigned int num_batches):
136  edge_batch_map(edge_batch_map), num_batches(num_batches)
137  {}
138  bool operator()(const Edge & e) const
139  {
140  return get(edge_batch_map, e) < (int)num_batches;
141  }
142  };
143 
144 private:
145  // part 2: members
146 
147  ompl_lemur::UtilityCheckerPtr _utility_checker;
148 
149  std::map<std::string, boost::function<Roadmap<RoadmapArgs> * (RoadmapArgs args)> > _roadmap_registry;
150 
151  std::string _roadmap_type;
152  std::vector<std::string> _roadmap_params;
153 
154  boost::shared_ptr< Roadmap<RoadmapArgs> > _roadmap;
155 
156 public:
157 
158  // danger, don't change this during planning!
159  boost::shared_ptr< TagCache<VIdxTagMap,EIdxTagsMap> > _tag_cache;
160 
161 private:
162 
163  std::vector< std::pair<size_t,size_t> > _subgraph_sizes; // numverts,numedges (cumulative)
164 
165  const ompl::base::StateSpacePtr space;
166  double check_radius; // this is half the standard resolution
167 
168  Graph g;
170  OverGraph og;
171 
172  OverVertex ov_singlestart;
173  OverVertex ov_singlegoal;
174 
175  // hack to account for singleroot edges given
176  // undirected representation (breaks heuristic consistency)
177  double _singlestart_cost;
178  double _singlegoal_cost;
179 
180  // for overlay anchors
181  std::map<Vertex, OverVertex> map_to_overlay;
182 
184  boost::property_map<OverGraph, Vertex OverVProps::*>::type,
185  boost::property_map<OverGraph, Edge OverEProps::*>::type>
186  overlay_manager;
187 
188  BisectPerm bisect_perm;
189 
190  // note: the nearest neighbor object will only store core roadmap vertices
191  // (not overlayed vertices), and will only be queried by the roadmap generator
192  // (not when e.g. connecting overlay vertices to new things, etc)
193  // eventually, it would be nice if we add/removed overlay vertices here as well!
194  boost::shared_ptr<NN> nn;
195 
196  // parameters
197  double _coeff_distance;
198  double _coeff_checkcost;
199  double _coeff_batch;
200 
201  bool _do_timing;
202  bool _persist_roots;
203 
204  unsigned int _num_batches_init; // dont do a search on batches below this
205  unsigned int _max_batches;
206 
207  unsigned int _num_batches_searched; // the number of batches currently being searched
208 
209  bool _solve_all;
210 
211  enum
212  {
213  SEARCH_TYPE_DIJKSTRAS,
214  SEARCH_TYPE_ASTAR,
215  SEARCH_TYPE_LPASTAR,
216  SEARCH_TYPE_RLPASTAR,
217  SEARCH_TYPE_INCBI,
218  SEARCH_TYPE_WINCBI
219  } _search_type;
220 
221  double _search_incbi_heur_interp;
222 
223  enum
224  {
225  SEARCH_INCBI_BALANCER_TYPE_DISTANCE,
226  SEARCH_INCBI_BALANCER_TYPE_CARDINALITY
227  } _search_incbi_balancer_type;
228 
229  double _search_incbi_balancer_goalfrac;
230 
231  enum
232  {
233  EVAL_TYPE_FWD,
234  EVAL_TYPE_REV,
235  EVAL_TYPE_ALT,
236  EVAL_TYPE_EVEN,
237  EVAL_TYPE_BISECT,
238  EVAL_TYPE_FWD_EXPAND,
239 #if 0
240  EVAL_TYPE_PARTITION_ALL,
241  EVAL_TYPE_SP_INDICATOR_PROBABILITY
242 #endif
243  } _eval_type;
244 
245 public:
246  std::ostream * os_alglog;
247 
248 private:
249  // property maps
250  VIdxTagMap _vidx_tag_map;
251  EIdxTagsMap _eidx_tags_map;
252 
253  boost::chrono::high_resolution_clock::duration _dur_total;
254  boost::chrono::high_resolution_clock::duration _dur_roadmapgen;
255  boost::chrono::high_resolution_clock::duration _dur_roadmapinit;
256  boost::chrono::high_resolution_clock::duration _dur_lazysp;
257  boost::chrono::high_resolution_clock::duration _dur_search;
258  boost::chrono::high_resolution_clock::duration _dur_eval;
259  boost::chrono::high_resolution_clock::duration _dur_selector_init;
260  boost::chrono::high_resolution_clock::duration _dur_selector;
261  boost::chrono::high_resolution_clock::duration _dur_selector_notify;
262 
263  // part 3: ompl methods
264 
265 public:
266  LEMUR(const ompl::base::SpaceInformationPtr & si);
267  ~LEMUR(void);
268 
269  template <template<class> class RoadmapTemplate>
270  void registerRoadmapType(std::string roadmap_type)
271  {
272  registerRoadmapType(roadmap_type, ompl_lemur::RoadmapFactory<RoadmapArgs,RoadmapTemplate>());
273  }
274 
275  void registerRoadmapType(std::string roadmap_type,
276  boost::function<Roadmap<RoadmapArgs> * (RoadmapArgs args)> factory);
277 
278  boost::shared_ptr< const Roadmap<RoadmapArgs> > getRoadmap();
279 
280  void setRoadmapType(std::string roadmap_type);
281  std::string getRoadmapType() const;
282 
283  void setCoeffDistance(double coeff_distance);
284  double getCoeffDistance() const;
285 
286  void setCoeffCheckcost(double coeff_checkcost);
287  double getCoeffCheckcost() const;
288 
289  void setCoeffBatch(double coeff_batch);
290  double getCoeffBatch() const;
291 
292  void setDoTiming(bool do_timing);
293  bool getDoTiming() const;
294 
295  void setPersistRoots(bool persist_roots);
296  bool getPersistRoots() const;
297 
298  void setNumBatchesInit(unsigned int max_batches);
299  unsigned int getNumBatchesInit() const;
300 
301  void setMaxBatches(unsigned int max_batches);
302  unsigned int getMaxBatches() const;
303 
304  void setSolveAll(bool solve_all);
305  bool getSolveAll() const;
306 
307  void setSearchType(std::string search_type);
308  std::string getSearchType() const;
309 
310  void setSearchIncbiHeurInterp(double search_incbi_heur_interp);
311  double getSearchIncbiHeurInterp() const;
312 
313  void setSearchIncbiBalancerType(std::string search_incbi_balancer_type);
314  std::string getSearchIncbiBalancerType() const;
315 
316  void setSearchIncbiBalancerGoalfrac(double search_incbi_balancer_goalfrac);
317  double getSearchIncbiBalancerGoalfrac() const;
318 
319  void setEvalType(std::string eval_type);
320  std::string getEvalType() const;
321 
322  // this is guaranteed to initialize the roadmap
323  void setProblemDefinition(const ompl::base::ProblemDefinitionPtr & pdef);
324 
325  template <class MyGraph, class IncSP, class EvalStrategy>
326  bool do_lazysp_c(MyGraph & graph, std::vector<Edge> & epath, IncSP incsp, EvalStrategy evalstrategy);
327 
328  template <class MyGraph, class IncSP>
329  bool do_lazysp_b(MyGraph & graph, std::vector<Edge> & epath, IncSP incsp);
330 
331  template <class MyGraph>
332  bool do_lazysp_a(MyGraph & graph, std::vector<Edge> & epath);
333 
334  // this fails if problem definition not set
336 
337  void dump_graph(std::ostream & os_graph);
338 
339  void saveTagCache();
340 
341  double getDurTotal();
342  double getDurRoadmapGen();
343  double getDurRoadmapInit();
344  double getDurLazySP();
345  double getDurSearch();
346  double getDurEval();
347  double getDurSelectorInit();
348  double getDurSelector();
349  double getDurSelectorNotify();
350 
351  // part 4: private methods
352 private:
353 
354  // assuming num_edge_states != edge_states.size(),
355  // this generates the states and their tags (assumed tag=0)
356  void edge_init_states(const Edge & e);
357 
358  void overlay_apply();
359  void overlay_unapply();
360 
361  void calculate_w_lazy(const Edge & e);
362 
363  // these are public so the property map wrappers can access them;
364  // instead, i should probable move those classes inside LEMUR
365 public:
366  bool isevaledmap_get(const Edge & e);
367  std::pair<double, std::vector<Edge> > wmap_get(const Edge & e);
368 
369 private:
370  double nn_dist(const Vertex & va, const Vertex & vb);
371 };
372 
373 // helper property map which delegates to FamilyPlanner::isevaledmap_get()
375 {
376 public:
377  typedef boost::readable_property_map_tag category;
378  typedef LEMUR::Edge key_type;
379  typedef bool value_type;
380  typedef bool reference;
381  LEMUR & lemur;
382  IsEvaledMap(LEMUR & lemur): lemur(lemur) {}
383 };
384 inline const double get(const IsEvaledMap & isevaledmap, const LEMUR::Edge & e)
385 {
386  return isevaledmap.lemur.isevaledmap_get(e);
387 }
388 
389 // helper property map which delegates to FamilyPlanner::wmap_get()
390 class WMap
391 {
392 public:
393  typedef boost::readable_property_map_tag category;
394  typedef LEMUR::Edge key_type;
395  typedef std::pair<double, std::vector<LEMUR::Edge> > value_type;
396  typedef std::pair<double, std::vector<LEMUR::Edge> > reference;
397  LEMUR & lemur;
398  WMap(LEMUR & lemur): lemur(lemur) {}
399 };
400 inline const std::pair<double, std::vector<LEMUR::Edge> > get(const WMap & wmap, const LEMUR::Edge & e)
401 {
402  return wmap.lemur.wmap_get(e);
403 }
404 
405 } // namespace ompl_lemur
Definition: LEMUR.h:32
Definition: LEMUR.h:96
Definition: NearestNeighborsLinearBGL.h:11
Definition: LEMUR.h:390
This uses pr_bgl::lazysp() and pr_bgl::incbi !
Definition: LEMUR.h:18
Definition: LEMUR.h:374
Definition: Roadmap.h:174
Definition: BisectPerm.h:12
Definition: LEMUR.h:23
Definition: Roadmap.h:11
Definition: LEMUR.h:105