Loading...
Searching...
No Matches
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
54namespace 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
151
154 {
155 return numFreeVolSamples_;
156 }
157
160 void setNumFreeVolumeSamples(int numSamples)
161 {
162 numFreeVolSamples_ = numSamples;
163 }
164
168 {
169 return probShortestPath_;
170 }
171
174 void setProbShortestPathLead(double probability)
175 {
176 probShortestPath_ = probability;
177 }
178
182 {
184 }
185
188 void setProbAddingToAvailableRegions(double probability)
189 {
190 probKeepAddingToAvail_ = probability;
191 }
192
196 {
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
224 {
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;
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;
308 double weight;
310 double alpha;
312 int index;
314 unsigned int numSelections;
317 };
318
322 {
323 public:
324 Adjacency() = default;
325 virtual ~Adjacency() = default;
327 void clear()
328 {
329 covGridCells.clear();
330 }
333 std::set<int> covGridCells;
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 class that will hold data contained in the PDF.
Definition PDF.h:53
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
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 int getDimension() const
Returns the dimension of this 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.
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds 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
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
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
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
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
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.
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.
STL namespace.
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()
Contains default values for Syclop parameters.
Definition Syclop.h:238