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, "10000:10000:500000");
110  Planner::declareParam<int> ("num_region_expansions", this, &Syclop::setNumRegionExpansions, &Syclop::getNumRegionExpansions, "10:10:500");
111  Planner::declareParam<int> ("num_tree_expansions", this, &Syclop::setNumTreeExpansions, &Syclop::getNumTreeExpansions, "0:1:100");
112  Planner::declareParam<double>("prob_abandon_lead_early", this, &Syclop::setProbAbandonLeadEarly, &Syclop::getProbAbandonLeadEarly, "0.:.05:1.");
113  Planner::declareParam<double>("prob_add_available_regions", this, &Syclop::setProbAddingToAvailableRegions, &Syclop::getProbAddingToAvailableRegions, "0.:.05:1.");
114  Planner::declareParam<double>("prob_shortest_path_lead", this, &Syclop::setProbShortestPathLead, &Syclop::getProbShortestPathLead, "0.:.05:1.");
115  }
116 
117  virtual ~Syclop()
118  {
119  }
120 
123 
124  virtual void setup();
125 
126  virtual void clear();
127 
132 
135 
137  void setLeadComputeFn(const LeadComputeFn& compute);
138 
140  void addEdgeCostFactor(const EdgeCostFactorFn& factor);
141 
143  void clearEdgeCostFactors();
144 
147  {
148  return numFreeVolSamples_;
149  }
150 
153  void setNumFreeVolumeSamples (int numSamples)
154  {
155  numFreeVolSamples_ = numSamples;
156  }
157 
160  double getProbShortestPathLead () 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 
189  {
190  return numRegionExpansions_;
191  }
192 
195  void setNumRegionExpansions (int regionExpansions)
196  {
197  numRegionExpansions_ = regionExpansions;
198  }
199 
202  int getNumTreeExpansions () const
203  {
204  return numTreeSelections_;
205  }
206 
209  void setNumTreeExpansions (int treeExpansions)
210  {
211  numTreeSelections_ = treeExpansions;
212  }
213 
216  double getProbAbandonLeadEarly () 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 = 1;
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.50*/;
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() : 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()
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()
281  {
282  }
283  virtual ~Region()
284  {
285  }
287  void clear()
288  {
289  motions.clear();
290  covGridCells.clear();
291  pdfElem = NULL;
292  }
293 
295  std::set<int> covGridCells;
297  std::vector<Motion*> motions;
299  double volume;
301  double freeVolume;
305  double weight;
307  double alpha;
309  int index;
311  unsigned int numSelections;
314  };
315  #pragma pack (pop) // Restoring default byte alignment
316 
317  #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
318 
320  class Adjacency
321  {
322  public:
323  Adjacency()
324  {
325  }
326  virtual ~Adjacency()
327  {
328  }
330  void clear()
331  {
332  covGridCells.clear();
333  }
336  std::set<int> covGridCells;
338  const Region *source;
340  const Region *target;
342  double cost;
349  bool empty;
350  };
351  #pragma pack (pop) // Restoring default byte alignment
352 
354  virtual Motion* addRoot(const base::State *s) = 0;
355 
358  virtual void selectAndExtend(Region &region, std::vector<Motion*>& newMotions) = 0;
359 
361  inline const Region& getRegionFromIndex(const int rid) const
362  {
363  return graph_[boost::vertex(rid,graph_)];
364  }
365 
368 
371 
374 
377 
380 
383 
386 
389 
392 
393  private:
396  class CoverageGrid : public GridDecomposition
397  {
398  public:
399  CoverageGrid(const int len, const DecompositionPtr& d) : GridDecomposition(len, d->getDimension(), d->getBounds()), decomp(d)
400  {
401  }
402 
403  virtual ~CoverageGrid()
404  {
405  }
406 
409  virtual void project(const base::State *s, std::vector<double>& coord) const
410  {
411  decomp->project(s, coord);
412  }
413 
415  virtual void sampleFullState(const base::StateSamplerPtr& /*sampler*/, const std::vector<double>& /*coord*/, base::State* /*s*/) const
416  {
417  }
418 
419  protected:
420  const DecompositionPtr& decomp;
421  };
422 
423  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
424  typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
425  typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
426  typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
427  typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;
428 
430  friend class DecompositionHeuristic;
431 
432  class DecompositionHeuristic : public boost::astar_heuristic<RegionGraph, double>
433  {
434  public:
435  DecompositionHeuristic(const Syclop *s, const Region &goal) : syclop(s), goalRegion(goal)
436  {
437  }
438 
439  double operator()(Vertex v)
440  {
441  const Region &region = syclop->getRegionFromIndex(v);
442  return region.alpha*goalRegion.alpha;
443  }
444  private:
445  const Syclop *syclop;
446  const Region &goalRegion;
447  };
448 
449  struct found_goal {};
450 
451  class GoalVisitor : public boost::default_astar_visitor
452  {
453  public:
454  GoalVisitor(const int goal) : goalRegion(goal)
455  {
456  }
457  void examine_vertex(Vertex v, const RegionGraph& /*g*/)
458  {
459  if (static_cast<int>(v) == goalRegion)
460  throw found_goal();
461  }
462  private:
463  const int goalRegion;
464  };
466 
468  class RegionSet
469  {
470  public:
471  int sampleUniform()
472  {
473  if (empty())
474  return -1;
475  return regions.sample(rng.uniform01());
476  }
477  void insert(const int r)
478  {
479  if (regToElem.count(r) == 0)
480  regToElem[r] = regions.add(r, 1);
481  else
482  {
483  PDF<int>::Element *elem = regToElem[r];
484  regions.update(elem, regions.getWeight(elem)+1);
485  }
486  }
487  void clear()
488  {
489  regions.clear();
490  regToElem.clear();
491  }
492  std::size_t size() const
493  {
494  return regions.size();
495  }
496  bool empty() const
497  {
498  return regions.empty();
499  }
500  private:
501  RNG rng;
502  PDF<int> regions;
503  boost::unordered_map<const int, PDF<int>::Element*> regToElem;
504  };
506 
508  void initRegion(Region &r);
509 
511  void setupRegionEstimates();
512 
514  void updateRegion(Region &r);
515 
517  void initEdge(Adjacency &a, const Region *source, const Region *target);
518 
520  void setupEdgeEstimates();
521 
523  void updateEdge(Adjacency &a);
524 
527  bool updateCoverageEstimate(Region &r, const base::State *s);
528 
531  bool updateConnectionEstimate(const Region &c, const Region &d, const base::State *s);
532 
535  void buildGraph();
536 
538  void clearGraphDetails();
539 
541  int selectRegion();
542 
544  void computeAvailableRegions();
545 
547  void defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead);
548 
550  double defaultEdgeCost(int r, int s);
551 
553  LeadComputeFn leadComputeFn;
555  std::vector<int> lead_;
557  PDF<int> availDist_;
559  std::vector<EdgeCostFactorFn> edgeCostFactors_;
561  CoverageGrid covGrid_;
563  RegionGraph graph_;
565  bool graphReady_;
567  boost::unordered_map<std::pair<int,int>, Adjacency*> regionsToEdge_;
569  unsigned int numMotions_;
571  RegionSet startRegions_;
573  RegionSet goalRegions_;
574  };
575  }
576 }
577 
578 #endif
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
Planner(const SpaceInformationPtr &si, const std::string &name)
Constructor.
Definition: Planner.cpp:43
Synergistic Combination of Layers of Planning.
Definition: Syclop.h:70
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:48
double alpha
The coefficient contributed by this region to edge weights in lead computations.
Definition: Syclop.h:307
base::State * state
The state contained by the motion.
Definition: Syclop.h:265
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
Definition: Syclop.h:320
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions from this region.
Definition: Syclop.h:295
int numLeadInclusions
The number of times this adjacency has been included in a lead.
Definition: Syclop.h:344
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
Definition: Syclop.h:347
Definition of an abstract control.
Definition: Control.h:48
int index
The index of the graph node corresponding to this region.
Definition: Syclop.h:309
RNG rng_
Random number generator.
Definition: Syclop.h:391
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
Definition: Syclop.h:313
A boost shared pointer wrapper for ompl::base::StateSampler.
double volume
The volume of this region.
Definition: Syclop.h:299
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:277
int getNumTreeExpansions() const
Get the number of calls to selectAndExtend() in the low-level tree planner for a given lead and regio...
Definition: Syclop.h:202
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: Syclop.h:258
std::vector< Motion * > motions
The tree motions contained in this region.
Definition: Syclop.h:297
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double getProbAbandonLeadEarly() const
Get the probability [0,1] that a lead will be abandoned early, before a new region is chosen for expa...
Definition: Syclop.h:216
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Definition: Syclop.h:385
DecompositionPtr decomp_
The high level decomposition used to focus tree expansion.
Definition: Syclop.h:388
A GridDecomposition is a Decomposition implemented using a grid.
Representation of a motion.
Definition: Syclop.h:251
int numTreeSelections_
The number of calls to selectAndExtend() in the low-level tree planner for a given lead and region...
Definition: Syclop.h:379
Control * control
The control contained by the motion.
Definition: Syclop.h:267
void clear()
Clears motions and coverage information from this region.
Definition: Syclop.h:287
double getProbShortestPathLead() const
Get the probability [0,1] that a lead will be computed as a shortest-path instead of a random-DFS...
Definition: Syclop.h:160
int numRegionExpansions_
The number of times a new region will be chosen and promoted for expansion from a given lead...
Definition: Syclop.h:376
boost::function< void(int, int, std::vector< int > &)> LeadComputeFn
Leads should consist of a path of adjacent regions in the decomposition that start with the start reg...
Definition: Syclop.h:91
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:57
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
void clearEdgeCostFactors()
Clears all edge cost factors, making all edge weights equivalent to 1.
Definition: Syclop.cpp:221
const Region & getRegionFromIndex(const int rid) const
Returns a reference to the Region object with the given index. Assumes the index is valid...
Definition: Syclop.h:361
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continues solving until a solution is found or a given planner termination condition is met...
Definition: Syclop.cpp:68
Syclop(const SpaceInformationPtr &si, const DecompositionPtr &d, const std::string &plannerName)
Constructor. Requires a Decomposition, which Syclop uses to create high-level leads.
Definition: Syclop.h:94
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
Base class for a planner.
Definition: Planner.h:232
void addEdgeCostFactor(const EdgeCostFactorFn &factor)
Adds an edge cost factor to be used for edge weights between adjacent regions.
Definition: Syclop.cpp:216
bool empty
This value is true if and only if this adjacency's source and target regions both contain zero tree m...
Definition: Syclop.h:349
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions originating from direct connectio...
Definition: Syclop.h:336
const Region * source
The source region of this adjacency edge.
Definition: Syclop.h:338
void setProbAbandonLeadEarly(double probability)
The probability that a lead will be abandoned early, before a new region is chosen for expansion...
Definition: Syclop.h:223
double cost
The cost of this adjacency edge, used in lead computations.
Definition: Syclop.h:342
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
const Region * target
The target region of this adjacency edge.
Definition: Syclop.h:340
boost::function< double(int, int)> EdgeCostFactorFn
Each edge weight between two adjacent regions in the Decomposition is defined as a product of edge co...
Definition: Syclop.h:88
Definition of an abstract state.
Definition: State.h:50
double percentValidCells
The percent of free volume of this region.
Definition: Syclop.h:303
A boost shared pointer wrapper for ompl::control::SpaceInformation.
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
double probShortestPath_
The probability that a lead will be computed as a shortest-path instead of a random-DFS.
Definition: Syclop.h:370
Contains default values for Syclop parameters.
Definition: Syclop.h:230
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
Definition: Syclop.cpp:211
int getNumRegionExpansions() const
Get the number of times a new region will be chosen and promoted for expansion from a given lead...
Definition: Syclop.h:188
unsigned int numSelections
The number of times this region has been selected for expansion.
Definition: Syclop.h:311
void setNumFreeVolumeSamples(int numSamples)
Set the number of states to sample when estimating free volume in the Decomposition.
Definition: Syclop.h:153
void setProbShortestPathLead(double probability)
Set the probability [0,1] that a lead will be computed as a shortest-path instead of a random-DFS...
Definition: Syclop.h:167
double probKeepAddingToAvail_
The probability that the set of available regions will be augmented.
Definition: Syclop.h:373
int getNumFreeVolumeSamples() const
Get the number of states to sample when estimating free volume in the Decomposition.
Definition: Syclop.h:146
virtual void selectAndExtend(Region &region, std::vector< Motion * > &newMotions)=0
Select a Motion from the given Region, and extend the tree from the Motion. Add any new motions creat...
A boost shared pointer wrapper for ompl::control::Decomposition.
double getProbAddingToAvailableRegions() const
Get the probability [0,1] that the set of available regions will be augmented.
Definition: Syclop.h:174
const Motion * parent
The parent motion in the tree.
Definition: Syclop.h:269
void clear()
Clears coverage information from this adjacency.
Definition: Syclop.h:330
double freeVolume
The free volume of this region.
Definition: Syclop.h:301
Space information containing necessary information for planning with controls. setup() needs to be ca...
int numFreeVolSamples_
The number of states to sample to estimate free volume in the Decomposition.
Definition: Syclop.h:367
void setNumTreeExpansions(int treeExpansions)
Set the number of calls to selectAndExtend() in the low-level tree planner for a given lead and regio...
Definition: Syclop.h:209
double probAbandonLeadEarly_
The probability that a lead will be abandoned early, before a new region is chosen for expansion...
Definition: Syclop.h:382
void setNumRegionExpansions(int regionExpansions)
Set the number of times a new region will be chosen and promoted for expansion from a given lead...
Definition: Syclop.h:195
double weight
The probabilistic weight of this region, used when sampling from PDF.
Definition: Syclop.h:305
virtual Motion * addRoot(const base::State *s)=0
Add State s as a new root in the low-level tree, and return the Motion corresponding to s...
void setProbAddingToAvailableRegions(double probability)
Set the probability [0,1] that the set of available regions will be augmented.
Definition: Syclop.h:181
unsigned int steps
The number of steps for which the control is applied.
Definition: Syclop.h:271