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