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 <unordered_map>
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 "ompl/util/Hash.h"
49 #include <functional>
50 #include <map>
51 #include <utility>
52 #include <vector>
53 
54 namespace ompl
55 {
56  namespace control
57  {
73  class Syclop : public base::Planner
74  {
75  public:
96  typedef std::function<double(int, int)> EdgeCostFactorFn;
97 
100  typedef std::function<void(int, int, std::vector<int> &)> LeadComputeFn;
101 
103  Syclop(const SpaceInformationPtr &si, DecompositionPtr d, const std::string &plannerName)
104  : ompl::base::Planner(si, plannerName)
105  , numFreeVolSamples_(Defaults::NUM_FREEVOL_SAMPLES)
106  , probShortestPath_(Defaults::PROB_SHORTEST_PATH)
107  , probKeepAddingToAvail_(Defaults::PROB_KEEP_ADDING_TO_AVAIL)
108  , numRegionExpansions_(Defaults::NUM_REGION_EXPANSIONS)
109  , numTreeSelections_(Defaults::NUM_TREE_SELECTIONS)
110  , probAbandonLeadEarly_(Defaults::PROB_ABANDON_LEAD_EARLY)
111  , siC_(si.get())
112  , decomp_(std::move(d))
113  , covGrid_(Defaults::COVGRID_LENGTH, decomp_)
114  , graphReady_(false)
115  , numMotions_(0)
116  {
118 
119  Planner::declareParam<int>("free_volume_samples", this, &Syclop::setNumFreeVolumeSamples,
120  &Syclop::getNumFreeVolumeSamples, "10000:10000:500000");
121  Planner::declareParam<int>("num_region_expansions", this, &Syclop::setNumRegionExpansions,
122  &Syclop::getNumRegionExpansions, "10:10:500");
123  Planner::declareParam<int>("num_tree_expansions", this, &Syclop::setNumTreeExpansions,
124  &Syclop::getNumTreeExpansions, "0:1:100");
125  Planner::declareParam<double>("prob_abandon_lead_early", this, &Syclop::setProbAbandonLeadEarly,
126  &Syclop::getProbAbandonLeadEarly, "0.:.05:1.");
127  Planner::declareParam<double>("prob_add_available_regions", this,
130  Planner::declareParam<double>("prob_shortest_path_lead", this, &Syclop::setProbShortestPathLead,
131  &Syclop::getProbShortestPathLead, "0.:.05:1.");
132  }
133 
134  ~Syclop() override = default;
135 
138 
139  void setup() override;
140 
141  void clear() override;
142 
147 
150 
152  void setLeadComputeFn(const LeadComputeFn &compute);
153 
155  void addEdgeCostFactor(const EdgeCostFactorFn &factor);
156 
158  void clearEdgeCostFactors();
159 
162  {
163  return numFreeVolSamples_;
164  }
165 
168  void setNumFreeVolumeSamples(int numSamples)
169  {
170  numFreeVolSamples_ = numSamples;
171  }
172 
175  double getProbShortestPathLead() const
176  {
177  return probShortestPath_;
178  }
179 
182  void setProbShortestPathLead(double probability)
183  {
184  probShortestPath_ = probability;
185  }
186 
190  {
191  return probKeepAddingToAvail_;
192  }
193 
196  void setProbAddingToAvailableRegions(double probability)
197  {
198  probKeepAddingToAvail_ = probability;
199  }
200 
204  {
205  return numRegionExpansions_;
206  }
207 
210  void setNumRegionExpansions(int regionExpansions)
211  {
212  numRegionExpansions_ = regionExpansions;
213  }
214 
218  {
219  return numTreeSelections_;
220  }
221 
224  void setNumTreeExpansions(int treeExpansions)
225  {
226  numTreeSelections_ = treeExpansions;
227  }
228 
231  double getProbAbandonLeadEarly() const
232  {
233  return probAbandonLeadEarly_;
234  }
235 
238  void setProbAbandonLeadEarly(double probability)
239  {
240  probAbandonLeadEarly_ = probability;
241  }
243 
245  struct Defaults
246  {
247  static const int NUM_FREEVOL_SAMPLES = 100000;
248  static const int COVGRID_LENGTH = 128;
249  static const int NUM_REGION_EXPANSIONS = 100;
250  static const int NUM_TREE_SELECTIONS = 1;
251  // C++ standard prohibits non-integral static const member initialization
252  // These constants are set in Syclop.cpp. C++11 standard changes this
253  // with the constexpr keyword, but for compatibility this is not done.
254  static const double PROB_ABANDON_LEAD_EARLY /*= 0.25*/;
255  static const double PROB_KEEP_ADDING_TO_AVAIL /*= 0.50*/;
256  static const double PROB_SHORTEST_PATH /*= 0.95*/;
257  };
258 
259  protected:
260 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
261 
265  class Motion
266  {
267  public:
268  Motion() : state(nullptr), control(nullptr), parent(nullptr), steps(0)
269  {
270  }
273  : state(si->allocState()), control(si->allocControl()), parent(nullptr), steps(0)
274  {
275  }
276  virtual ~Motion() = default;
282  const Motion *parent;
284  unsigned int steps;
285  };
286 #pragma pack(pop) // Restoring default byte alignment
287 
288 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
289 
290  class Region
291  {
292  public:
293  Region()
294  {
295  }
296  virtual ~Region() = default;
297 
298 #if __cplusplus >= 201103L
299  Region(const Region &) = default;
300  Region &operator=(const Region &) = default;
301  Region(Region &&) = default;
302  Region &operator=(Region &&) = default;
303 #endif
304 
306  void clear()
307  {
308  motions.clear();
309  covGridCells.clear();
310  pdfElem = nullptr;
311  }
312 
314  std::set<int> covGridCells;
316  std::vector<Motion *> motions;
318  double volume;
320  double freeVolume;
324  double weight;
326  double alpha;
328  int index;
330  unsigned int numSelections;
333  };
334 #pragma pack(pop) // Restoring default byte alignment
335 
336 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
337 
339  class Adjacency
340  {
341  public:
342  Adjacency()
343  {
344  }
345  virtual ~Adjacency() = default;
347  void clear()
348  {
349  covGridCells.clear();
350  }
353  std::set<int> covGridCells;
355  const Region *source;
357  const Region *target;
359  double cost;
367  bool empty;
368  };
369 #pragma pack(pop) // Restoring default byte alignment
370 
372  virtual Motion *addRoot(const base::State *s) = 0;
373 
376  virtual void selectAndExtend(Region &region, std::vector<Motion *> &newMotions) = 0;
377 
379  inline const Region &getRegionFromIndex(const int rid) const
380  {
381  return graph_[boost::vertex(rid, graph_)];
382  }
383 
386 
389 
392 
395 
399 
403 
406 
409 
412 
413  private:
415 
416  struct HashRegionPair
417  {
418  size_t operator()(const std::pair<int, int> &p) const
419  {
420  std::size_t hash = std::hash<int>()(p.first);
421  hash_combine(hash, p.second);
422  return hash;
423  }
424  };
426 
429  class CoverageGrid : public GridDecomposition
430  {
431  public:
432  CoverageGrid(const int len, const DecompositionPtr &d)
433  : GridDecomposition(len, d->getDimension(), d->getBounds()), decomp(d)
434  {
435  }
436 
437  ~CoverageGrid() override = default;
438 
441  void project(const base::State *s, std::vector<double> &coord) const override
442  {
443  decomp->project(s, coord);
444  }
445 
447  void sampleFullState(const base::StateSamplerPtr & /*sampler*/, const std::vector<double> & /*coord*/,
448  base::State * /*s*/) const override
449  {
450  }
451 
452  protected:
453  const DecompositionPtr &decomp;
454  };
455 
456  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
457  typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
458  typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
459  typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
460  typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;
461 
463  friend class DecompositionHeuristic;
464 
465  class DecompositionHeuristic : public boost::astar_heuristic<RegionGraph, double>
466  {
467  public:
468  DecompositionHeuristic(const Syclop *s, const Region &goal) : syclop(s), goalRegion(goal)
469  {
470  }
471 
472  double operator()(Vertex v)
473  {
474  const Region &region = syclop->getRegionFromIndex(v);
475  return region.alpha * goalRegion.alpha;
476  }
477 
478  private:
479  const Syclop *syclop;
480  const Region &goalRegion;
481  };
482 
483  struct found_goal
484  {
485  };
486 
487  class GoalVisitor : public boost::default_astar_visitor
488  {
489  public:
490  GoalVisitor(const int goal) : goalRegion(goal)
491  {
492  }
493  void examine_vertex(Vertex v, const RegionGraph & /*g*/)
494  {
495  if (static_cast<int>(v) == goalRegion)
496  throw found_goal();
497  }
498 
499  private:
500  const int goalRegion;
501  };
503 
505  class RegionSet
506  {
507  public:
508  int sampleUniform()
509  {
510  if (empty())
511  return -1;
512  return regions.sample(rng.uniform01());
513  }
514  void insert(const int r)
515  {
516  if (regToElem.count(r) == 0)
517  regToElem[r] = regions.add(r, 1);
518  else
519  {
520  PDF<int>::Element *elem = regToElem[r];
521  regions.update(elem, regions.getWeight(elem) + 1);
522  }
523  }
524  void clear()
525  {
526  regions.clear();
527  regToElem.clear();
528  }
529  std::size_t size() const
530  {
531  return regions.size();
532  }
533  bool empty() const
534  {
535  return regions.empty();
536  }
537 
538  private:
539  RNG rng;
540  PDF<int> regions;
541  std::unordered_map<int, PDF<int>::Element *> regToElem;
542  };
544 
546  void initRegion(Region &r);
547 
549  void setupRegionEstimates();
550 
552  void updateRegion(Region &r);
553 
555  void initEdge(Adjacency &a, const Region *source, const Region *target);
556 
558  void setupEdgeEstimates();
559 
561  void updateEdge(Adjacency &a);
562 
565  bool updateCoverageEstimate(Region &r, const base::State *s);
566 
569  bool updateConnectionEstimate(const Region &c, const Region &d, const base::State *s);
570 
573  void buildGraph();
574 
576  void clearGraphDetails();
577 
579  int selectRegion();
580 
582  void computeAvailableRegions();
583 
586  void defaultComputeLead(int startRegion, int goalRegion, std::vector<int> &lead);
587 
589  double defaultEdgeCost(int r, int s);
590 
592  LeadComputeFn leadComputeFn;
594  std::vector<int> lead_;
596  PDF<int> availDist_;
598  std::vector<EdgeCostFactorFn> edgeCostFactors_;
600  CoverageGrid covGrid_;
603  RegionGraph graph_;
605  bool graphReady_;
607  std::unordered_map<std::pair<int, int>, Adjacency *, HashRegionPair> regionsToEdge_;
609  unsigned int numMotions_;
611  RegionSet startRegions_;
613  RegionSet goalRegions_;
614  };
615  }
616 }
617 
618 #endif
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
Synergistic Combination of Layers of Planning.
Definition: Syclop.h:73
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:175
double alpha
The coefficient contributed by this region to edge weights in lead computations.
Definition: Syclop.h:326
base::State * state
The state contained by the motion.
Definition: Syclop.h:278
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
Definition: Syclop.h:339
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions from this region.
Definition: Syclop.h:314
int numLeadInclusions
The number of times this adjacency has been included in a lead.
Definition: Syclop.h:361
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
Definition: Syclop.h:364
Definition of an abstract control.
Definition: Control.h:47
int index
The index of the graph node corresponding to this region.
Definition: Syclop.h:328
RNG rng_
Random number generator.
Definition: Syclop.h:411
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
Definition: Syclop.h:332
A shared pointer wrapper for ompl::base::StateSampler.
double volume
The volume of this region.
Definition: Syclop.h:318
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:290
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: Syclop.h:272
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Definition: Syclop.h:405
STL namespace.
double getProbAddingToAvailableRegions() const
Get the probability [0,1] that the set of available regions will be augmented.
Definition: Syclop.h:189
DecompositionPtr decomp_
The high level decomposition used to focus tree expansion.
Definition: Syclop.h:408
A GridDecomposition is a Decomposition implemented using a grid.
Representation of a motion.
Definition: Syclop.h:265
int numTreeSelections_
The number of calls to selectAndExtend() in the low-level tree planner for a given lead and region...
Definition: Syclop.h:398
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:217
Control * control
The control contained by the motion.
Definition: Syclop.h:280
void clear()
Clears motions and coverage information from this region.
Definition: Syclop.h:306
std::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:100
int numRegionExpansions_
The number of times a new region will be chosen and promoted for expansion from a given lead...
Definition: Syclop.h:394
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:227
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:48
int getNumFreeVolumeSamples() const
Get the number of states to sample when estimating free volume in the Decomposition.
Definition: Syclop.h:161
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
double getWeight(const Element *elem) const
Returns the current weight of the given Element.
Definition: PDF.h:171
Base class for a planner.
Definition: Planner.h:232
Syclop(const SpaceInformationPtr &si, DecompositionPtr d, const std::string &plannerName)
Constructor. Requires a Decomposition, which Syclop uses to create high-level leads.
Definition: Syclop.h:103
void addEdgeCostFactor(const EdgeCostFactorFn &factor)
Adds an edge cost factor to be used for edge weights between adjacent regions.
Definition: Syclop.cpp:222
bool empty
This value is true if and only if this adjacency&#39;s source and target regions both contain zero tree m...
Definition: Syclop.h:367
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions originating from direct connectio...
Definition: Syclop.h:353
const Region * source
The source region of this adjacency edge.
Definition: Syclop.h:355
std::vector< Motion * > motions
The tree motions contained in this region.
Definition: Syclop.h:316
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:203
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continues solving until a solution is found or a given planner termination condition is met...
Definition: Syclop.cpp:74
std::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:96
void setProbAbandonLeadEarly(double probability)
The probability that a lead will be abandoned early, before a new region is chosen for expansion...
Definition: Syclop.h:238
double cost
The cost of this adjacency edge, used in lead computations.
Definition: Syclop.h:359
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:357
Definition of an abstract state.
Definition: State.h:49
double percentValidCells
The percent of free volume of this region.
Definition: Syclop.h:322
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:379
A 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:427
double probShortestPath_
The probability that a lead will be computed as a shortest-path instead of a random-DFS.
Definition: Syclop.h:388
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:231
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
Contains default values for Syclop parameters.
Definition: Syclop.h:245
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
Definition: Syclop.cpp:217
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:63
unsigned int numSelections
The number of times this region has been selected for expansion.
Definition: Syclop.h:330
void setNumFreeVolumeSamples(int numSamples)
Set the number of states to sample when estimating free volume in the Decomposition.
Definition: Syclop.h:168
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:182
double probKeepAddingToAvail_
The probability that the set of available regions will be augmented.
Definition: Syclop.h:391
A shared pointer wrapper for ompl::control::Decomposition.
const Motion * parent
The parent motion in the tree.
Definition: Syclop.h:282
void clear()
Clears coverage information from this adjacency.
Definition: Syclop.h:347
double freeVolume
The free volume of this region.
Definition: Syclop.h:320
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:385
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:224
double probAbandonLeadEarly_
The probability that a lead will be abandoned early, before a new region is chosen for expansion...
Definition: Syclop.h:402
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...
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:210
double weight
The probabilistic weight of this region, used when sampling from PDF.
Definition: Syclop.h:324
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:196
unsigned int steps
The number of steps for which the control is applied.
Definition: Syclop.h:284