All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
Syclop.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Matt Maly */
36 
37 #ifndef OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
38 #define OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
39 
40 #include <boost/graph/astar_search.hpp>
41 #include <boost/graph/graph_traits.hpp>
42 #include <boost/graph/adjacency_list.hpp>
43 #include <boost/unordered_map.hpp>
44 #include "ompl/control/planners/PlannerIncludes.h"
45 #include "ompl/control/planners/syclop/Decomposition.h"
46 #include "ompl/control/planners/syclop/GridDecomposition.h"
47 #include "ompl/datastructures/PDF.h"
48 #include <map>
49 #include <vector>
50 
51 namespace ompl
52 {
53  namespace control
54  {
70  class Syclop : public base::Planner
71  {
72  public:
88  typedef boost::function<double(int, int)> EdgeCostFactorFn;
89 
91  typedef boost::function<void(int, int, std::vector<int>&)> LeadComputeFn;
92 
94  Syclop(const SpaceInformationPtr& si, const DecompositionPtr &d, const std::string& plannerName) : ompl::base::Planner(si, plannerName),
95  numFreeVolSamples_(Defaults::NUM_FREEVOL_SAMPLES),
96  probShortestPath_(Defaults::PROB_SHORTEST_PATH),
97  probKeepAddingToAvail_(Defaults::PROB_KEEP_ADDING_TO_AVAIL),
98  numRegionExpansions_(Defaults::NUM_REGION_EXPANSIONS),
99  numTreeSelections_(Defaults::NUM_TREE_SELECTIONS),
100  probAbandonLeadEarly_(Defaults::PROB_ABANDON_LEAD_EARLY),
101  siC_(si.get()),
102  decomp_(d),
103  covGrid_(Defaults::COVGRID_LENGTH, decomp_),
104  graphReady_(false),
105  numMotions_(0)
106  {
108 
109  Planner::declareParam<int> ("free_volume_samples", this, &Syclop::setNumFreeVolumeSamples, &Syclop::getNumFreeVolumeSamples);
110  Planner::declareParam<int> ("num_region_expansions", this, &Syclop::setNumRegionExpansions, &Syclop::getNumRegionExpansions);
111  Planner::declareParam<int> ("num_tree_expansions", this, &Syclop::setNumTreeExpansions, &Syclop::getNumTreeExpansions);
112  Planner::declareParam<double>("prob_abandon_lead_early", this, &Syclop::setProbAbandonLeadEarly, &Syclop::getProbAbandonLeadEarly);
113  Planner::declareParam<double>("prob_add_available_regions", this, &Syclop::setProbAddingToAvailableRegions, &Syclop::getProbAddingToAvailableRegions);
114  Planner::declareParam<double>("prob_shortest_path_lead", this, &Syclop::setProbShortestPathLead, &Syclop::getProbShortestPathLead);
115  }
116 
117  virtual ~Syclop()
118  {
119  }
120 
123 
124  virtual void setup(void);
125 
126  virtual void clear(void);
127 
132 
135 
137  void setLeadComputeFn(const LeadComputeFn& compute);
138 
140  void addEdgeCostFactor(const EdgeCostFactorFn& factor);
141 
143  void clearEdgeCostFactors(void);
144 
146  int getNumFreeVolumeSamples (void) const
147  {
148  return numFreeVolSamples_;
149  }
150 
153  void setNumFreeVolumeSamples (int numSamples)
154  {
155  numFreeVolSamples_ = numSamples;
156  }
157 
160  double getProbShortestPathLead (void) const
161  {
162  return probShortestPath_;
163  }
164 
167  void setProbShortestPathLead (double probability)
168  {
169  probShortestPath_ = probability;
170  }
171 
175  {
176  return probKeepAddingToAvail_;
177  }
178 
181  void setProbAddingToAvailableRegions (double probability)
182  {
183  probKeepAddingToAvail_ = probability;
184  }
185 
188  int getNumRegionExpansions (void) const
189  {
190  return numRegionExpansions_;
191  }
192 
195  void setNumRegionExpansions (int regionExpansions)
196  {
197  numRegionExpansions_ = regionExpansions;
198  }
199 
202  int getNumTreeExpansions (void) const
203  {
204  return numTreeSelections_;
205  }
206 
209  void setNumTreeExpansions (int treeExpansions)
210  {
211  numTreeSelections_ = treeExpansions;
212  }
213 
216  double getProbAbandonLeadEarly (void) const
217  {
218  return probAbandonLeadEarly_;
219  }
220 
223  void setProbAbandonLeadEarly (double probability)
224  {
225  probAbandonLeadEarly_ = probability;
226  }
228 
230  struct Defaults
231  {
232  static const int NUM_FREEVOL_SAMPLES = 100000;
233  static const int COVGRID_LENGTH = 128;
234  static const int NUM_REGION_EXPANSIONS = 100;
235  static const int NUM_TREE_SELECTIONS = 50;
236  // C++ standard prohibits non-integral static const member initialization
237  // These constants are set in Syclop.cpp. C++11 standard changes this
238  // with the constexpr keyword, but for compatibility this is not done.
239  static const double PROB_ABANDON_LEAD_EARLY /*= 0.25*/;
240  static const double PROB_KEEP_ADDING_TO_AVAIL /*= 0.95*/;
241  static const double PROB_SHORTEST_PATH /*= 0.95*/;
242  };
243 
244  protected:
245 
246  #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
247 
251  class Motion
252  {
253  public:
254  Motion(void) : state(NULL), control(NULL), parent(NULL), steps(0)
255  {
256  }
258  Motion(const SpaceInformation* si) : state(si->allocState()), control(si->allocControl()), parent(NULL), steps(0)
259  {
260  }
261  virtual ~Motion(void)
262  {
263  }
269  const Motion* parent;
271  unsigned int steps;
272  };
273  #pragma pack (pop) // Restoring default byte alignment
274 
275  #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
276 
277  class Region
278  {
279  public:
280  Region(void)
281  {
282  }
283  virtual ~Region(void)
284  {
285  }
287  void clear(void)
288  {
289  motions.clear();
290  covGridCells.clear();
291  }
292 
294  std::set<int> covGridCells;
296  std::vector<Motion*> motions;
298  double volume;
300  double freeVolume;
304  double weight;
306  double alpha;
308  int index;
310  unsigned int numSelections;
311  };
312  #pragma pack (pop) // Restoring default byte alignment
313 
314  #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
315 
317  class Adjacency
318  {
319  public:
320  Adjacency(void)
321  {
322  }
323  virtual ~Adjacency(void)
324  {
325  }
327  void clear(void)
328  {
329  covGridCells.clear();
330  }
333  std::set<int> covGridCells;
335  const Region* source;
337  const Region* target;
339  double cost;
346  bool empty;
347  };
348  #pragma pack (pop) // Restoring default byte alignment
349 
351  virtual Motion* addRoot(const base::State* s) = 0;
352 
355  virtual void selectAndExtend(Region& region, std::vector<Motion*>& newMotions) = 0;
356 
358  inline const Region& getRegionFromIndex(const int rid) const
359  {
360  return graph_[boost::vertex(rid,graph_)];
361  }
362 
365 
368 
371 
374 
377 
380 
383 
386 
389 
390  private:
393  class CoverageGrid : public GridDecomposition
394  {
395  public:
396  CoverageGrid(const int len, const DecompositionPtr& d) : GridDecomposition(len, d->getDimension(), d->getBounds()), decomp(d)
397  {
398  }
399 
400  virtual ~CoverageGrid()
401  {
402  }
403 
406  virtual void project(const base::State* s, std::vector<double>& coord) const
407  {
408  decomp->project(s, coord);
409  }
410 
412  virtual void sampleFromRegion(const int rid, const base::StateSamplerPtr& sampler, base::State* s)
413  {
414  }
415 
416  protected:
417  const DecompositionPtr& decomp;
418  };
419 
420  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
421  typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
422  typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
423  typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
424  typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;
425 
427  friend class DecompositionHeuristic;
428 
429  class DecompositionHeuristic : public boost::astar_heuristic<RegionGraph, double>
430  {
431  public:
432  DecompositionHeuristic(const Syclop* s, const Region& goal) : syclop(s), goalRegion(goal)
433  {
434  }
435 
436  double operator()(Vertex v)
437  {
438  const Region& region = syclop->getRegionFromIndex(v);
439  return region.alpha*goalRegion.alpha;
440  }
441  private:
442  const Syclop* syclop;
443  const Region& goalRegion;
444  };
445 
446  struct found_goal {};
447 
448  class GoalVisitor : public boost::default_astar_visitor
449  {
450  public:
451  GoalVisitor(const unsigned int goal) : goalRegion(goal)
452  {
453  }
454  void examine_vertex(Vertex v, const RegionGraph& g)
455  {
456  if (v == goalRegion)
457  throw found_goal();
458  }
459  private:
460  const unsigned int goalRegion;
461  };
463 
465  class RegionSet
466  {
467  public:
468  int sampleUniform(void)
469  {
470  if (empty())
471  return -1;
472  return regions.sample(rng.uniform01());
473  }
474  void insert(const int r)
475  {
476  if (regToElem.count(r) == 0)
477  regToElem[r] = regions.add(r, 1);
478  else
479  {
480  PDF<int>::Element* elem = regToElem[r];
481  regions.update(elem, regions.getWeight(elem)+1);
482  }
483  }
484  void clear(void)
485  {
486  regions.clear();
487  regToElem.clear();
488  }
489  std::size_t size(void) const
490  {
491  return regions.size();
492  }
493  bool empty() const
494  {
495  return regions.empty();
496  }
497  private:
498  RNG rng;
499  PDF<int> regions;
500  boost::unordered_map<const int, PDF<int>::Element*> regToElem;
501  };
503 
505  void initRegion(Region& r);
506 
508  void setupRegionEstimates(void);
509 
511  void updateRegion(Region& r);
512 
514  void initEdge(Adjacency& a, const Region* source, const Region* target);
515 
517  void setupEdgeEstimates(void);
518 
520  void updateEdge(Adjacency& a);
521 
524  bool updateCoverageEstimate(Region& r, const base::State* s);
525 
528  bool updateConnectionEstimate(const Region& c, const Region& d, const base::State* s);
529 
532  void buildGraph(void);
533 
535  void clearGraphDetails(void);
536 
538  int selectRegion(void);
539 
541  void computeAvailableRegions(void);
542 
544  void defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead);
545 
547  double defaultEdgeCost(int r, int s);
548 
550  LeadComputeFn leadComputeFn;
552  std::vector<int> lead_;
554  PDF<int> availDist_;
556  std::vector<EdgeCostFactorFn> edgeCostFactors_;
558  CoverageGrid covGrid_;
560  RegionGraph graph_;
562  bool graphReady_;
564  boost::unordered_map<std::pair<int,int>, Adjacency*> regionsToEdge_;
566  unsigned int numMotions_;
568  RegionSet startRegions_;
570  RegionSet goalRegions_;
571  };
572  }
573 }
574 
575 #endif