Syclop.cpp
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 #include "ompl/control/planners/syclop/Syclop.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/base/ProblemDefinition.h"
40 #include <limits>
41 #include <stack>
42 #include <algorithm>
43 
44 const double ompl::control::Syclop::Defaults::PROB_ABANDON_LEAD_EARLY = 0.25;
45 const double ompl::control::Syclop::Defaults::PROB_KEEP_ADDING_TO_AVAIL = 0.50;
46 const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH = 0.95;
47 
49 {
51  if (!leadComputeFn)
52  setLeadComputeFn([this](int startRegion, int goalRegion, std::vector<int> &lead)
53  {
54  defaultComputeLead(startRegion, goalRegion, lead);
55  });
56  buildGraph();
57  addEdgeCostFactor([this](int r, int s)
58  {
59  return defaultEdgeCost(r, s);
60  });
61 }
62 
64 {
66  lead_.clear();
67  availDist_.clear();
69  clearGraphDetails();
70  startRegions_.clear();
71  goalRegions_.clear();
72 }
73 
75 {
76  checkValidity();
77  if (!graphReady_)
78  {
79  numMotions_ = 0;
80  setupRegionEstimates();
81  setupEdgeEstimates();
82  graphReady_ = true;
83  }
84  while (const base::State *s = pis_.nextStart())
85  {
86  const int region = decomp_->locateRegion(s);
87  startRegions_.insert(region);
88  Motion *startMotion = addRoot(s);
89  graph_[boost::vertex(region, graph_)].motions.push_back(startMotion);
90  ++numMotions_;
91  updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s);
92  }
93  if (startRegions_.empty())
94  {
95  OMPL_ERROR("%s: There are no valid start states", getName().c_str());
97  }
98 
99  // We need at least one valid goal sample so that we can find the goal region
100  if (goalRegions_.empty())
101  {
102  if (const base::State *g = pis_.nextGoal(ptc))
103  goalRegions_.insert(decomp_->locateRegion(g));
104  else
105  {
106  OMPL_ERROR("%s: Unable to sample a valid goal state", getName().c_str());
108  }
109  }
110 
111  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
112 
113  std::vector<Motion *> newMotions;
114  const Motion *solution = nullptr;
115  base::Goal *goal = pdef_->getGoal().get();
116  double goalDist = std::numeric_limits<double>::infinity();
117  bool solved = false;
118  while (!ptc && !solved)
119  {
120  const int chosenStartRegion = startRegions_.sampleUniform();
121  int chosenGoalRegion = -1;
122 
123  // if we have not sampled too many goal regions already
124  if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_ / 2)
125  {
126  if (const base::State *g = pis_.nextGoal())
127  {
128  chosenGoalRegion = decomp_->locateRegion(g);
129  goalRegions_.insert(chosenGoalRegion);
130  }
131  }
132  if (chosenGoalRegion == -1)
133  chosenGoalRegion = goalRegions_.sampleUniform();
134 
135  leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
136  computeAvailableRegions();
137  for (int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i)
138  {
139  const int region = selectRegion();
140  bool improved = false;
141  for (int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j)
142  {
143  newMotions.clear();
144  selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions);
145  for (std::vector<Motion *>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
146  {
147  Motion *motion = *m;
148  double distance;
149  solved = goal->isSatisfied(motion->state, &distance);
150  if (solved)
151  {
152  goalDist = distance;
153  solution = motion;
154  break;
155  }
156 
157  // Check for approximate (best-so-far) solution
158  if (distance < goalDist)
159  {
160  goalDist = distance;
161  solution = motion;
162  }
163  const int newRegion = decomp_->locateRegion(motion->state);
164  graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion);
165  ++numMotions_;
166  Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)];
167  improved |= updateCoverageEstimate(newRegionObj, motion->state);
168  /* If tree has just crossed from one region to its neighbor,
169  update the connection estimates. If the tree has crossed an entire region,
170  then region and newRegion are not adjacent, and so we do not update estimates. */
171  if (newRegion != region && regionsToEdge_.count(std::pair<int, int>(region, newRegion)) > 0)
172  {
173  Adjacency *adj = regionsToEdge_[std::pair<int, int>(region, newRegion)];
174  adj->empty = false;
175  ++adj->numSelections;
176  improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj,
177  motion->state);
178  }
179 
180  /* If this region already exists in availDist, update its weight. */
181  if (newRegionObj.pdfElem != nullptr)
182  availDist_.update(newRegionObj.pdfElem, newRegionObj.weight);
183  /* Otherwise, only add this region to availDist
184  if it already exists in the lead. */
185  else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
186  {
187  PDF<int>::Element *elem = availDist_.add(newRegion, newRegionObj.weight);
188  newRegionObj.pdfElem = elem;
189  }
190  }
191  }
192  if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
193  break;
194  }
195  }
196  bool addedSolution = false;
197  if (solution != nullptr)
198  {
199  std::vector<const Motion *> mpath;
200  while (solution != nullptr)
201  {
202  mpath.push_back(solution);
203  solution = solution->parent;
204  }
205  auto path(std::make_shared<PathControl>(si_));
206  for (int i = mpath.size() - 1; i >= 0; --i)
207  if (mpath[i]->parent != nullptr)
208  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
209  else
210  path->append(mpath[i]->state);
211  pdef_->addSolutionPath(path, !solved, goalDist, getName());
212  addedSolution = true;
213  }
215 }
216 
218 {
219  leadComputeFn = compute;
220 }
221 
223 {
224  edgeCostFactors_.push_back(factor);
225 }
226 
228 {
229  edgeCostFactors_.clear();
230 }
231 
232 void ompl::control::Syclop::initRegion(Region &r)
233 {
234  r.numSelections = 0;
235  r.volume = 1.0;
236  r.percentValidCells = 1.0;
237  r.freeVolume = 1.0;
238  r.pdfElem = nullptr;
239 }
240 
241 void ompl::control::Syclop::setupRegionEstimates()
242 {
243  std::vector<int> numTotal(decomp_->getNumRegions(), 0);
244  std::vector<int> numValid(decomp_->getNumRegions(), 0);
245  base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
246  base::StateSamplerPtr sampler = si_->allocStateSampler();
247  base::State *s = si_->allocState();
248 
249  for (int i = 0; i < numFreeVolSamples_; ++i)
250  {
251  sampler->sampleUniform(s);
252  int rid = decomp_->locateRegion(s);
253  if (rid >= 0)
254  {
255  if (checker->isValid(s))
256  ++numValid[rid];
257  ++numTotal[rid];
258  }
259  }
260  si_->freeState(s);
261 
262  for (int i = 0; i < decomp_->getNumRegions(); ++i)
263  {
264  Region &r = graph_[boost::vertex(i, graph_)];
265  r.volume = decomp_->getRegionVolume(i);
266  if (numTotal[i] == 0)
267  r.percentValidCells = 1.0;
268  else
269  r.percentValidCells = ((double)numValid[i]) / (double)numTotal[i];
271  if (r.freeVolume < std::numeric_limits<double>::epsilon())
272  r.freeVolume = std::numeric_limits<double>::epsilon();
273  updateRegion(r);
274  }
275 }
276 
277 void ompl::control::Syclop::updateRegion(Region &r)
278 {
279  const double f = r.freeVolume * r.freeVolume * r.freeVolume * r.freeVolume;
280  r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
281  r.weight = f / ((1 + r.covGridCells.size()) * (1 + r.numSelections * r.numSelections));
282 }
283 
284 void ompl::control::Syclop::initEdge(Adjacency &adj, const Region *source, const Region *target)
285 {
286  adj.source = source;
287  adj.target = target;
288  updateEdge(adj);
289  regionsToEdge_[std::pair<int, int>(source->index, target->index)] = &adj;
290 }
291 
292 void ompl::control::Syclop::setupEdgeEstimates()
293 {
294  EdgeIter ei, eend;
295  for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
296  {
297  Adjacency &adj = graph_[*ei];
298  adj.empty = true;
299  adj.numLeadInclusions = 0;
300  adj.numSelections = 0;
301  updateEdge(adj);
302  }
303 }
304 
305 void ompl::control::Syclop::updateEdge(Adjacency &a)
306 {
307  a.cost = 1.0;
308  for (const auto &factor : edgeCostFactors_)
309  {
310  a.cost *= factor(a.source->index, a.target->index);
311  }
312 }
313 
314 bool ompl::control::Syclop::updateCoverageEstimate(Region &r, const base::State *s)
315 {
316  const int covCell = covGrid_.locateRegion(s);
317  if (r.covGridCells.count(covCell) == 1)
318  return false;
319  r.covGridCells.insert(covCell);
320  updateRegion(r);
321  return true;
322 }
323 
324 bool ompl::control::Syclop::updateConnectionEstimate(const Region &c, const Region &d, const base::State *s)
325 {
326  Adjacency &adj = *regionsToEdge_[std::pair<int, int>(c.index, d.index)];
327  const int covCell = covGrid_.locateRegion(s);
328  if (adj.covGridCells.count(covCell) == 1)
329  return false;
330  adj.covGridCells.insert(covCell);
331  updateEdge(adj);
332  return true;
333 }
334 
335 void ompl::control::Syclop::buildGraph()
336 {
337  VertexIndexMap index = get(boost::vertex_index, graph_);
338  std::vector<int> neighbors;
339  for (int i = 0; i < decomp_->getNumRegions(); ++i)
340  {
341  const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
342  Region &r = graph_[boost::vertex(v, graph_)];
343  initRegion(r);
344  r.index = index[v];
345  }
346  VertexIter vi, vend;
347  for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
348  {
349  /* Create an edge between this vertex and each of its neighboring regions in the decomposition,
350  and initialize the edge's Adjacency object. */
351  decomp_->getNeighbors(index[*vi], neighbors);
352  for (const auto &j : neighbors)
353  {
354  RegionGraph::edge_descriptor edge;
355  bool ignore;
356  boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(j, graph_), graph_);
357  initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(j, graph_)]);
358  }
359  neighbors.clear();
360  }
361 }
362 
363 void ompl::control::Syclop::clearGraphDetails()
364 {
365  VertexIter vi, vend;
366  for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
367  graph_[*vi].clear();
368  EdgeIter ei, eend;
369  for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
370  graph_[*ei].clear();
371  graphReady_ = false;
372 }
373 
374 int ompl::control::Syclop::selectRegion()
375 {
376  const int index = availDist_.sample(rng_.uniform01());
377  Region &region = graph_[boost::vertex(index, graph_)];
378  ++region.numSelections;
379  updateRegion(region);
380  return index;
381 }
382 
383 void ompl::control::Syclop::computeAvailableRegions()
384 {
385  for (unsigned int i = 0; i < availDist_.size(); ++i)
386  graph_[boost::vertex(availDist_[i], graph_)].pdfElem = nullptr;
387  availDist_.clear();
388  for (int i = lead_.size() - 1; i >= 0; --i)
389  {
390  Region &r = graph_[boost::vertex(lead_[i], graph_)];
391  if (!r.motions.empty())
392  {
393  r.pdfElem = availDist_.add(lead_[i], r.weight);
395  break;
396  }
397  }
398 }
399 
400 void ompl::control::Syclop::defaultComputeLead(int startRegion, int goalRegion, std::vector<int> &lead)
401 {
402  lead.clear();
403  if (startRegion == goalRegion)
404  {
405  lead.push_back(startRegion);
406  return;
407  }
408 
410  {
411  std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
412  std::vector<double> distances(decomp_->getNumRegions());
413 
414  try
415  {
416  boost::astar_search(graph_, boost::vertex(startRegion, graph_),
417  DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
418  boost::weight_map(get(&Adjacency::cost, graph_))
419  .distance_map(boost::make_iterator_property_map(distances.begin(),
420  get(boost::vertex_index, graph_)))
421  .predecessor_map(boost::make_iterator_property_map(
422  parents.begin(), get(boost::vertex_index, graph_)))
423  .visitor(GoalVisitor(goalRegion)));
424  }
425  catch (found_goal fg)
426  {
427  int region = goalRegion;
428  int leadLength = 1;
429 
430  while (region != startRegion)
431  {
432  region = parents[region];
433  ++leadLength;
434  }
435  lead.resize(leadLength);
436  region = goalRegion;
437  for (int i = leadLength - 1; i >= 0; --i)
438  {
439  lead[i] = region;
440  region = parents[region];
441  }
442  }
443  }
444  else
445  {
446  /* Run a random-DFS over the decomposition graph from the start region to the goal region.
447  There may be a way to do this using boost::depth_first_search. */
448  VertexIndexMap index = get(boost::vertex_index, graph_);
449  std::stack<int> nodesToProcess;
450  std::vector<int> parents(decomp_->getNumRegions(), -1);
451  parents[startRegion] = startRegion;
452  nodesToProcess.push(startRegion);
453  bool goalFound = false;
454  while (!goalFound && !nodesToProcess.empty())
455  {
456  const int v = nodesToProcess.top();
457  nodesToProcess.pop();
458  std::vector<int> neighbors;
459  boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
460  for (boost::tie(ai, aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai)
461  {
462  if (parents[index[*ai]] < 0)
463  {
464  neighbors.push_back(index[*ai]);
465  parents[index[*ai]] = v;
466  }
467  }
468  for (std::size_t i = 0; i < neighbors.size(); ++i)
469  {
470  const int choice = rng_.uniformInt(i, neighbors.size() - 1);
471  if (neighbors[choice] == goalRegion)
472  {
473  int region = goalRegion;
474  int leadLength = 1;
475  while (region != startRegion)
476  {
477  region = parents[region];
478  ++leadLength;
479  }
480  lead.resize(leadLength);
481  region = goalRegion;
482  for (int j = leadLength - 1; j >= 0; --j)
483  {
484  lead[j] = region;
485  region = parents[region];
486  }
487  goalFound = true;
488  break;
489  }
490  nodesToProcess.push(neighbors[choice]);
491  std::swap(neighbors[i], neighbors[choice]);
492  }
493  }
494  }
495 
496  // Now that we have a lead, update the edge weights.
497  for (std::size_t i = 0; i < lead.size() - 1; ++i)
498  {
499  Adjacency &adj = *regionsToEdge_[std::pair<int, int>(lead[i], lead[i + 1])];
500  if (adj.empty)
501  {
502  ++adj.numLeadInclusions;
503  updateEdge(adj);
504  }
505  }
506 }
507 
508 double ompl::control::Syclop::defaultEdgeCost(int r, int s)
509 {
510  const Adjacency &a = *regionsToEdge_[std::pair<int, int>(r, s)];
511  double factor = 1.0;
512  const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
513  factor = (1.0 + (double)nsel * nsel) / (1.0 + (double)a.covGridCells.size() * a.covGridCells.size());
514  factor *= (a.source->alpha * a.target->alpha);
515  return factor;
516 }
double alpha
The coefficient contributed by this region to edge weights in lead computations.
Definition: Syclop.h:315
base::State * state
The state contained by the motion.
Definition: Syclop.h:269
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
Definition: Syclop.h:328
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions from this region.
Definition: Syclop.h:303
int numLeadInclusions
The number of times this adjacency has been included in a lead.
Definition: Syclop.h:348
The planner failed to find a solution.
Definition: PlannerStatus.h:62
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
Definition: Syclop.h:351
std::size_t size() const
Returns the number of elements in the PDF.
Definition: PDF.h:251
int index
The index of the graph node corresponding to this region.
Definition: Syclop.h:317
RNG rng_
Random number generator.
Definition: Syclop.h:398
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
Definition: Syclop.h:321
A shared pointer wrapper for ompl::base::StateSampler.
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:264
double volume
The volume of this region.
Definition: Syclop.h:307
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:281
void clear()
Clears the PDF.
Definition: PDF.h:242
Abstract definition of goals.
Definition: Goal.h:62
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:392
DecompositionPtr decomp_
The high level decomposition used to focus tree expansion.
Definition: Syclop.h:395
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:113
Representation of a motion.
Definition: Syclop.h:257
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
int numTreeSelections_
The number of calls to selectAndExtend() in the low-level tree planner for a given lead and region...
Definition: Syclop.h:385
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:381
A shared pointer wrapper for ompl::base::StateValidityChecker.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
void clearEdgeCostFactors()
Clears all edge cost factors, making all edge weights equivalent to 1.
Definition: Syclop.cpp:227
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:48
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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:354
The planner found an exact solution.
Definition: PlannerStatus.h:66
std::set< int > covGridCells
The cells of the underlying coverage grid that contain tree motions originating from direct connectio...
Definition: Syclop.h:340
const Region * source
The source region of this adjacency edge.
Definition: Syclop.h:342
std::vector< Motion * > motions
The tree motions contained in this region.
Definition: Syclop.h:305
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
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:87
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
double cost
The cost of this adjacency edge, used in lead computations.
Definition: Syclop.h:346
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:344
Definition of an abstract state.
Definition: State.h:49
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
double percentValidCells
The percent of free volume of this region.
Definition: Syclop.h:311
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
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:366
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
double probShortestPath_
The probability that a lead will be computed as a shortest-path instead of a random-DFS.
Definition: Syclop.h:375
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:132
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
Definition: Syclop.cpp:217
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element, which can be used to subsequently update or remove the data from the PDF.
Definition: PDF.h:97
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:319
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:342
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
double probKeepAddingToAvail_
The probability that the set of available regions will be augmented.
Definition: Syclop.h:378
const Motion * parent
The parent motion in the tree.
Definition: Syclop.h:273
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
double freeVolume
The free volume of this region.
Definition: Syclop.h:309
int numFreeVolSamples_
The number of states to sample to estimate free volume in the Decomposition.
Definition: Syclop.h:372
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:81
double probAbandonLeadEarly_
The probability that a lead will be abandoned early, before a new region is chosen for expansion...
Definition: Syclop.h:389
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...
double weight
The probabilistic weight of this region, used when sampling from PDF.
Definition: Syclop.h:313
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...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68