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  using EdgeCostFactorFn = std::function<double(int, int)>;
97 
100  using LeadComputeFn = std::function<void(int, int, std::vector<int> &)>;
101 
103  Syclop(const SpaceInformationPtr &si, DecompositionPtr d, const std::string &plannerName)
104  : ompl::base::Planner(si, plannerName)
105  , siC_(si.get())
106  , decomp_(std::move(d))
107  , covGrid_(Defaults::COVGRID_LENGTH, decomp_)
108  {
110 
111  Planner::declareParam<int>("free_volume_samples", this, &Syclop::setNumFreeVolumeSamples,
112  &Syclop::getNumFreeVolumeSamples, "10000:10000:500000");
113  Planner::declareParam<int>("num_region_expansions", this, &Syclop::setNumRegionExpansions,
114  &Syclop::getNumRegionExpansions, "10:10:500");
115  Planner::declareParam<int>("num_tree_expansions", this, &Syclop::setNumTreeExpansions,
116  &Syclop::getNumTreeExpansions, "0:1:100");
117  Planner::declareParam<double>("prob_abandon_lead_early", this, &Syclop::setProbAbandonLeadEarly,
118  &Syclop::getProbAbandonLeadEarly, "0.:.05:1.");
119  Planner::declareParam<double>("prob_add_available_regions", this,
122  Planner::declareParam<double>("prob_shortest_path_lead", this, &Syclop::setProbShortestPathLead,
123  &Syclop::getProbShortestPathLead, "0.:.05:1.");
124  }
125 
126  ~Syclop() override = default;
127 
130 
131  void setup() override;
132 
133  void clear() override;
134 
139 
142 
144  void setLeadComputeFn(const LeadComputeFn &compute);
145 
147  void addEdgeCostFactor(const EdgeCostFactorFn &factor);
148 
150  void clearEdgeCostFactors();
151 
154  {
155  return numFreeVolSamples_;
156  }
157 
160  void setNumFreeVolumeSamples(int numSamples)
161  {
162  numFreeVolSamples_ = numSamples;
163  }
164 
167  double getProbShortestPathLead() const
168  {
169  return probShortestPath_;
170  }
171 
174  void setProbShortestPathLead(double probability)
175  {
176  probShortestPath_ = probability;
177  }
178 
182  {
183  return probKeepAddingToAvail_;
184  }
185 
188  void setProbAddingToAvailableRegions(double probability)
189  {
190  probKeepAddingToAvail_ = probability;
191  }
192 
196  {
197  return numRegionExpansions_;
198  }
199 
202  void setNumRegionExpansions(int regionExpansions)
203  {
204  numRegionExpansions_ = regionExpansions;
205  }
206 
210  {
211  return numTreeSelections_;
212  }
213 
216  void setNumTreeExpansions(int treeExpansions)
217  {
218  numTreeSelections_ = treeExpansions;
219  }
220 
223  double getProbAbandonLeadEarly() const
224  {
225  return probAbandonLeadEarly_;
226  }
227 
230  void setProbAbandonLeadEarly(double probability)
231  {
232  probAbandonLeadEarly_ = probability;
233  }
235 
237  struct Defaults
238  {
239  static const int NUM_FREEVOL_SAMPLES = 100000;
240  static const int COVGRID_LENGTH = 128;
241  static const int NUM_REGION_EXPANSIONS = 100;
242  static const int NUM_TREE_SELECTIONS = 1;
243  // C++ standard prohibits non-integral static const member initialization
244  // These constants are set in Syclop.cpp. C++11 standard changes this
245  // with the constexpr keyword, but for compatibility this is not done.
246  static const double PROB_ABANDON_LEAD_EARLY /*= 0.25*/;
247  static const double PROB_KEEP_ADDING_TO_AVAIL /*= 0.50*/;
248  static const double PROB_SHORTEST_PATH /*= 0.95*/;
249  };
250 
251  protected:
256  class Motion
257  {
258  public:
259  Motion() = default;
260 
263  : state(si->allocState()), control(si->allocControl())
264  {
265  }
266  virtual ~Motion() = default;
268  base::State *state{nullptr};
270  Control *control{nullptr};
272  const Motion *parent{nullptr};
274  unsigned int steps{0};
275  };
276 
278  class Region
279  {
280  public:
281  Region() = default;
282  virtual ~Region() = default;
283 
284  Region(const Region &) = default;
285  Region &operator=(const Region &) = default;
286  Region(Region &&) = default;
287  Region &operator=(Region &&) = default;
288 
290  void clear()
291  {
292  motions.clear();
293  covGridCells.clear();
294  pdfElem = nullptr;
295  }
296 
298  std::set<int> covGridCells;
300  std::vector<Motion *> motions;
302  double volume;
304  double freeVolume;
308  double weight;
310  double alpha;
312  int index;
314  unsigned int numSelections;
317  };
318 
321  class Adjacency
322  {
323  public:
324  Adjacency() = default;
325  virtual ~Adjacency() = default;
327  void clear()
328  {
329  covGridCells.clear();
330  }
333  std::set<int> covGridCells;
335  const Region *source;
337  const Region *target;
339  double cost;
347  bool empty;
348  };
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 
364  int numFreeVolSamples_{Defaults::NUM_FREEVOL_SAMPLES};
365 
367  double probShortestPath_{Defaults::PROB_SHORTEST_PATH};
368 
370  double probKeepAddingToAvail_{Defaults::PROB_KEEP_ADDING_TO_AVAIL};
371 
373  int numRegionExpansions_{Defaults::NUM_REGION_EXPANSIONS};
374 
377  int numTreeSelections_{Defaults::NUM_TREE_SELECTIONS};
378 
381  double probAbandonLeadEarly_{Defaults::PROB_ABANDON_LEAD_EARLY};
382 
385 
388 
391 
392  private:
394 
395  struct HashRegionPair
396  {
397  size_t operator()(const std::pair<int, int> &p) const
398  {
399  std::size_t hash = std::hash<int>()(p.first);
400  hash_combine(hash, p.second);
401  return hash;
402  }
403  };
405 
408  class CoverageGrid : public GridDecomposition
409  {
410  public:
411  CoverageGrid(const int len, const DecompositionPtr &d)
412  : GridDecomposition(len, d->getDimension(), d->getBounds()), decomp(d)
413  {
414  }
415 
416  ~CoverageGrid() override = default;
417 
420  void project(const base::State *s, std::vector<double> &coord) const override
421  {
422  decomp->project(s, coord);
423  }
424 
426  void sampleFullState(const base::StateSamplerPtr & /*sampler*/, const std::vector<double> & /*coord*/,
427  base::State * /*s*/) const override
428  {
429  }
430 
431  protected:
432  const DecompositionPtr &decomp;
433  };
434 
435  using RegionGraph = boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency>;
436  using Vertex = boost::graph_traits<RegionGraph>::vertex_descriptor;
437  using VertexIter = boost::graph_traits<RegionGraph>::vertex_iterator;
438  using VertexIndexMap = boost::property_map<RegionGraph, boost::vertex_index_t>::type;
439  using EdgeIter = boost::graph_traits<RegionGraph>::edge_iterator;
440 
442  friend class DecompositionHeuristic;
443 
444  class DecompositionHeuristic : public boost::astar_heuristic<RegionGraph, double>
445  {
446  public:
447  DecompositionHeuristic(const Syclop *s, const Region &goal) : syclop(s), goalRegion(goal)
448  {
449  }
450 
451  double operator()(Vertex v)
452  {
453  const Region &region = syclop->getRegionFromIndex(v);
454  return region.alpha * goalRegion.alpha;
455  }
456 
457  private:
458  const Syclop *syclop;
459  const Region &goalRegion;
460  };
461 
462  struct found_goal
463  {
464  };
465 
466  class GoalVisitor : public boost::default_astar_visitor
467  {
468  public:
469  GoalVisitor(const int goal) : goalRegion(goal)
470  {
471  }
472  void examine_vertex(Vertex v, const RegionGraph & /*g*/)
473  {
474  if (static_cast<int>(v) == goalRegion)
475  throw found_goal();
476  }
477 
478  private:
479  const int goalRegion;
480  };
482 
484  class RegionSet
485  {
486  public:
487  int sampleUniform()
488  {
489  if (empty())
490  return -1;
491  return regions.sample(rng.uniform01());
492  }
493  void insert(const int r)
494  {
495  if (regToElem.count(r) == 0)
496  regToElem[r] = regions.add(r, 1);
497  else
498  {
499  PDF<int>::Element *elem = regToElem[r];
500  regions.update(elem, regions.getWeight(elem) + 1);
501  }
502  }
503  void clear()
504  {
505  regions.clear();
506  regToElem.clear();
507  }
508  std::size_t size() const
509  {
510  return regions.size();
511  }
512  bool empty() const
513  {
514  return regions.empty();
515  }
516 
517  private:
518  RNG rng;
519  PDF<int> regions;
520  std::unordered_map<int, PDF<int>::Element *> regToElem;
521  };
523 
525  void initRegion(Region &r);
526 
528  void setupRegionEstimates();
529 
531  void updateRegion(Region &r);
532 
534  void initEdge(Adjacency &adj, const Region *source, const Region *target);
535 
537  void setupEdgeEstimates();
538 
540  void updateEdge(Adjacency &a);
541 
544  bool updateCoverageEstimate(Region &r, const base::State *s);
545 
548  bool updateConnectionEstimate(const Region &c, const Region &d, const base::State *s);
549 
552  void buildGraph();
553 
555  void clearGraphDetails();
556 
558  int selectRegion();
559 
561  void computeAvailableRegions();
562 
565  void defaultComputeLead(int startRegion, int goalRegion, std::vector<int> &lead);
566 
568  double defaultEdgeCost(int r, int s);
569 
571  LeadComputeFn leadComputeFn;
573  std::vector<int> lead_;
575  PDF<int> availDist_;
577  std::vector<EdgeCostFactorFn> edgeCostFactors_;
579  CoverageGrid covGrid_;
582  RegionGraph graph_;
584  bool graphReady_{false};
586  std::unordered_map<std::pair<int, int>, Adjacency *, HashRegionPair> regionsToEdge_;
588  unsigned int numMotions_{0};
590  RegionSet startRegions_;
592  RegionSet goalRegions_;
593  };
594  }
595 }
596 
597 #endif
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:49
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
Definition of an abstract control.
Definition: Control.h:48
A shared pointer wrapper for ompl::control::Decomposition.
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.
A GridDecomposition is a Decomposition implemented using a grid.
Space information containing necessary information for planning with controls. setup() needs to be ca...
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
Definition: Syclop.h:322
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions originating from direct connectio...
Definition: Syclop.h:333
const Region * target
The target region of this adjacency edge.
Definition: Syclop.h:337
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:347
int numLeadInclusions
The number of times this adjacency has been included in a lead.
Definition: Syclop.h:341
double cost
The cost of this adjacency edge, used in lead computations.
Definition: Syclop.h:339
const Region * source
The source region of this adjacency edge.
Definition: Syclop.h:335
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
Definition: Syclop.h:344
void clear()
Clears coverage information from this adjacency.
Definition: Syclop.h:327
Representation of a motion.
Definition: Syclop.h:257
unsigned int steps
The number of steps for which the control is applied.
Definition: Syclop.h:274
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: Syclop.h:262
const Motion * parent
The parent motion in the tree.
Definition: Syclop.h:272
base::State * state
The state contained by the motion.
Definition: Syclop.h:268
Control * control
The control contained by the motion.
Definition: Syclop.h:270
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:279
double volume
The volume of this region.
Definition: Syclop.h:302
double percentValidCells
The percent of free volume of this region.
Definition: Syclop.h:306
void clear()
Clears motions and coverage information from this region.
Definition: Syclop.h:290
std::vector< Motion * > motions
The tree motions contained in this region.
Definition: Syclop.h:300
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions from this region.
Definition: Syclop.h:298
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
Definition: Syclop.h:316
double alpha
The coefficient contributed by this region to edge weights in lead computations.
Definition: Syclop.h:310
double weight
The probabilistic weight of this region, used when sampling from PDF.
Definition: Syclop.h:308
unsigned int numSelections
The number of times this region has been selected for expansion.
Definition: Syclop.h:314
double freeVolume
The free volume of this region.
Definition: Syclop.h:304
int index
The index of the graph node corresponding to this region.
Definition: Syclop.h:312
Synergistic Combination of Layers of Planning.
Definition: Syclop.h:74
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:49
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:223
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:216
void setNumFreeVolumeSamples(int numSamples)
Set the number of states to sample when estimating free volume in the Decomposition.
Definition: Syclop.h:160
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Definition: Syclop.h:384
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 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:174
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 clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:64
double probKeepAddingToAvail_
The probability that the set of available regions will be augmented.
Definition: Syclop.h:370
double probShortestPath_
The probability that a lead will be computed as a shortest-path instead of a random-DFS.
Definition: Syclop.h:367
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:358
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 clearEdgeCostFactors()
Clears all edge cost factors, making all edge weights equivalent to 1.
Definition: Syclop.cpp:228
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:223
int numTreeSelections_
The number of calls to selectAndExtend() in the low-level tree planner for a given lead and region.
Definition: Syclop.h:377
double getProbAddingToAvailableRegions() const
Get the probability [0,1] that the set of available regions will be augmented.
Definition: Syclop.h:181
double probAbandonLeadEarly_
The probability that a lead will be abandoned early, before a new region is chosen for expansion.
Definition: Syclop.h:381
int getNumFreeVolumeSamples() const
Get the number of states to sample when estimating free volume in the Decomposition.
Definition: Syclop.h:153
void setProbAddingToAvailableRegions(double probability)
Set the probability [0,1] that the set of available regions will be augmented.
Definition: Syclop.h:188
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:167
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:202
int numRegionExpansions_
The number of times a new region will be chosen and promoted for expansion from a given lead.
Definition: Syclop.h:373
RNG rng_
Random number generator.
Definition: Syclop.h:390
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:209
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
void setProbAbandonLeadEarly(double probability)
The probability that a lead will be abandoned early, before a new region is chosen for expansion.
Definition: Syclop.h:230
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
Definition: Syclop.cpp:218
int numFreeVolSamples_
The number of states to sample to estimate free volume in the Decomposition.
Definition: Syclop.h:364
DecompositionPtr decomp_
The high level decomposition used to focus tree expansion.
Definition: Syclop.h:387
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:75
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:195
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
Contains default values for Syclop parameters.
Definition: Syclop.h:238