SPARS.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
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 Rutgers 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: Andrew Dobson */
36 
37 #include "ompl/geometric/planners/prm/SPARS.h"
38 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
41 #include "ompl/tools/config/SelfConfig.h"
42 #include "ompl/tools/config/MagicConstants.h"
43 #include <boost/bind.hpp>
44 #include <boost/graph/astar_search.hpp>
45 #include <boost/graph/incremental_components.hpp>
46 #include <boost/property_map/vector_property_map.hpp>
47 #include <boost/foreach.hpp>
48 
49 #include "GoalVisitor.hpp"
50 
51 #define foreach BOOST_FOREACH
52 #define foreach_reverse BOOST_REVERSE_FOREACH
53 
55  base::Planner(si, "SPARS"),
56  geomPath_(si),
57  stateProperty_(boost::get(vertex_state_t(), g_)),
58  sparseStateProperty_(boost::get(vertex_state_t(), s_)),
59  sparseColorProperty_(boost::get(vertex_color_t(), s_)),
60  representativesProperty_(boost::get(vertex_representative_t(), g_)),
61  nonInterfaceListsProperty_(boost::get(vertex_list_t(), s_)),
62  interfaceListsProperty_(boost::get(vertex_interface_list_t(), s_)),
63  weightProperty_(boost::get(boost::edge_weight, g_)),
64  sparseDJSets_(boost::get(boost::vertex_rank, s_),
65  boost::get(boost::vertex_predecessor, s_)),
66  consecutiveFailures_(0),
67  stretchFactor_(3.),
68  maxFailures_(1000),
69  addedSolution_(false),
70  denseDeltaFraction_(.001),
71  sparseDeltaFraction_(.25),
72  denseDelta_(0.),
73  sparseDelta_(0.),
74  iterations_(0),
75  bestCost_(std::numeric_limits<double>::quiet_NaN())
76 {
79  specs_.optimizingPaths = true;
80  specs_.multithreaded = true;
81 
82  psimp_.reset(new PathSimplifier(si_));
83  psimp_->freeStates(false);
84 
85  Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:3.0");
86  Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction, &SPARS::getSparseDeltaFraction, "0.0:0.01:1.0");
87  Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction, &SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1");
88  Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:3000");
89 
90  addPlannerProgressProperty("iterations INTEGER",
91  boost::bind(&SPARS::getIterationCount, this));
92  addPlannerProgressProperty("best cost REAL",
93  boost::bind(&SPARS::getBestCost, this));
94 }
95 
97 {
98  freeMemory();
99 }
100 
102 {
103  Planner::setup();
104  if (!nn_)
105  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(si_->getStateSpace()));
106  nn_->setDistanceFunction(boost::bind(&SPARS::distanceFunction, this, _1, _2));
107  if (!snn_)
108  snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(si_->getStateSpace()));
109  snn_->setDistanceFunction(boost::bind(&SPARS::sparseDistanceFunction, this, _1, _2));
110  if (!connectionStrategy_)
111  connectionStrategy_ = KStarStrategy<DenseVertex>(boost::bind(&SPARS::milestoneCount, this), nn_, si_->getStateDimension());
112  double maxExt = si_->getMaximumExtent();
113  sparseDelta_ = sparseDeltaFraction_ * maxExt;
114  denseDelta_ = denseDeltaFraction_ * maxExt;
115 
116  // Setup optimization objective
117  //
118  // If no optimization objective was specified, then default to
119  // optimizing path length as computed by the distance() function
120  // in the state space.
121  if (pdef_)
122  {
123  if (pdef_->hasOptimizationObjective())
124  {
125  opt_ = pdef_->getOptimizationObjective();
126  if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt_.get()))
127  OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str());
128  }
129  else
130  opt_.reset(new base::PathLengthOptimizationObjective(si_));
131  }
132  else
133  {
134  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
135  setup_ = false;
136  }
137 }
138 
140 {
141  Planner::setProblemDefinition(pdef);
142  clearQuery();
143 }
144 
146 {
147  consecutiveFailures_ = 0;
148 }
149 
151 {
152  startM_.clear();
153  goalM_.clear();
154  pis_.restart();
155 }
156 
158 {
159  Planner::clear();
160  sampler_.reset();
161  simpleSampler_.reset();
162  freeMemory();
163  if (nn_)
164  nn_->clear();
165  if (snn_)
166  snn_->clear();
167  clearQuery();
168  resetFailures();
169  iterations_ = 0;
170  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
171 }
172 
174 {
175  foreach (DenseVertex v, boost::vertices(g_))
176  if( stateProperty_[v] != NULL )
177  {
178  si_->freeState(stateProperty_[v]);
179  stateProperty_[v] = NULL;
180  }
181  foreach (SparseVertex n, boost::vertices(s_))
182  if( sparseStateProperty_[n] != NULL )
183  {
184  si_->freeState(sparseStateProperty_[n]);
185  sparseStateProperty_[n] = NULL;
186  }
187  s_.clear();
188  g_.clear();
189 }
190 
192 {
193  DenseVertex result = boost::graph_traits<DenseGraph>::null_vertex();
194 
195  // search for a valid state
196  bool found = false;
197  while (!found && ptc == false)
198  {
199  unsigned int attempts = 0;
200  do
201  {
202  found = sampler_->sample(workState);
203  attempts++;
204  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
205  }
206 
207  if (found)
208  result = addMilestone(si_->cloneState(workState));
209  return result;
210 }
211 
213 {
214  base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
215  while (!ptc && !addedSolution_)
216  {
217  // Check for any new goal states
218  if (goal->maxSampleCount() > goalM_.size())
219  {
220  const base::State *st = pis_.nextGoal();
221  if (st)
222  {
223  addMilestone(si_->cloneState(st));
224  goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
225  }
226  }
227 
228  // Check for a solution
229  addedSolution_ = haveSolution(startM_, goalM_, solution);
230  // Sleep for 1ms
231  if (!addedSolution_)
232  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
233  }
234 }
235 
236 bool ompl::geometric::SPARS::haveSolution(const std::vector<DenseVertex> &starts, const std::vector<DenseVertex> &goals, base::PathPtr &solution)
237 {
238  base::Goal *g = pdef_->getGoal().get();
239  base::Cost sol_cost(opt_->infiniteCost());
240  foreach (DenseVertex start, starts)
241  {
242  foreach (DenseVertex goal, goals)
243  {
244  // we lock because the connected components algorithm is incremental and may change disjointSets_
245  graphMutex_.lock();
246  bool same_component = sameComponent(start, goal);
247  graphMutex_.unlock();
248 
249  if (same_component && g->isStartGoalPairValid(sparseStateProperty_[goal], sparseStateProperty_[start]))
250  {
251  base::PathPtr p = constructSolution(start, goal);
252  if (p)
253  {
254  base::Cost pathCost = p->cost(opt_);
255  if (opt_->isCostBetterThan(pathCost, bestCost_))
256  bestCost_ = pathCost;
257  // Check if optimization objective is satisfied
258  if (opt_->isSatisfied(pathCost))
259  {
260  solution = p;
261  return true;
262  }
263  else if (opt_->isCostBetterThan(pathCost, sol_cost))
264  {
265  solution = p;
266  sol_cost = pathCost;
267  }
268  }
269  }
270  }
271  }
272 
273  return false;
274 }
275 
277 {
278  return consecutiveFailures_ >= maxFailures_ || addedSolution_;
279 }
280 
282 {
283  return consecutiveFailures_ >= maxFailures_;
284 }
285 
287 {
288  boost::mutex::scoped_lock _(graphMutex_);
289  if (boost::num_vertices(g_) < 1)
290  {
291  sparseQueryVertex_ = boost::add_vertex(s_);
292  queryVertex_ = boost::add_vertex(g_);
293  sparseStateProperty_[sparseQueryVertex_] = NULL;
294  stateProperty_[queryVertex_] = NULL;
295  }
296 }
297 
299 {
300  return boost::same_component(m1, m2, sparseDJSets_);
301 }
302 
304 {
305  checkValidity();
306  checkQueryStateInitialization();
307 
308  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
309 
310  if (!goal)
311  {
312  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
314  }
315 
316  // Add the valid start states as milestones
317  while (const base::State *st = pis_.nextStart())
318  {
319  addMilestone(si_->cloneState(st));
320  startM_.push_back(addGuard(si_->cloneState(st), START ));
321  }
322  if (startM_.empty())
323  {
324  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
326  }
327 
328  if (goalM_.empty() && !goal->couldSample())
329  {
330  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
332  }
333 
334  // Add the valid goal states as milestones
335  while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
336  {
337  addMilestone(si_->cloneState(st));
338  goalM_.push_back(addGuard(si_->cloneState(st), GOAL ));
339  }
340  if (goalM_.empty())
341  {
342  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
344  }
345 
346  unsigned int nrStartStatesDense = boost::num_vertices(g_) - 1; // don't count query vertex
347  unsigned int nrStartStatesSparse = boost::num_vertices(s_) - 1; // don't count query vertex
348  OMPL_INFORM("%s: Starting planning with %u dense states, %u sparse states", getName().c_str(), nrStartStatesDense, nrStartStatesSparse);
349 
350  // Reset addedSolution_ member
351  addedSolution_ = false;
352  resetFailures();
353  base::PathPtr sol;
356  boost::thread slnThread(boost::bind(&SPARS::checkForSolution, this, ptcOrFail, boost::ref(sol)));
357 
358  // Construct planner termination condition which also takes maxFailures_ and addedSolution_ into account
361  constructRoadmap(ptcOrStop);
362 
363  // Ensure slnThread is ceased before exiting solve
364  slnThread.join();
365 
366  if (sol)
367  pdef_->addSolutionPath(sol, false, -1.0, getName());
368 
369  OMPL_INFORM("%s: Created %u dense states, %u sparse states", getName().c_str(),
370  (unsigned int)(boost::num_vertices(g_) - nrStartStatesDense),
371  (unsigned int)(boost::num_vertices(s_) - nrStartStatesSparse));
372 
373  // Return true if any solution was found.
375 }
376 
378 {
379  if (stopOnMaxFail)
380  {
381  resetFailures();
384  constructRoadmap(ptcOrFail);
385  }
386  else
387  constructRoadmap(ptc);
388 }
389 
391 {
392  checkQueryStateInitialization();
393 
394  if (!isSetup())
395  setup();
396  if (!sampler_)
397  sampler_ = si_->allocValidStateSampler();
398  if (!simpleSampler_)
399  simpleSampler_ = si_->allocStateSampler();
400 
401  base::State *workState = si_->allocState();
402 
403  /* The whole neighborhood set which has been most recently computed */
404  std::vector<SparseVertex> graphNeighborhood;
405 
406  /* The visible neighborhood set which has been most recently computed */
407  std::vector<SparseVertex> visibleNeighborhood;
408 
409  /* Storage for the interface neighborhood, populated by getInterfaceNeighborhood() */
410  std::vector<DenseVertex> interfaceNeighborhood;
411 
412  bestCost_ = opt_->infiniteCost();
413  while (ptc == false)
414  {
415  iterations_++;
416 
417  // Generate a single sample, and attempt to connect it to nearest neighbors.
418  DenseVertex q = addSample(workState, ptc);
419  if (q == boost::graph_traits<DenseGraph>::null_vertex())
420  continue;
421 
422  //Now that we've added to D, try adding to S
423  //Start by figuring out who our neighbors are
424  getSparseNeighbors(workState, graphNeighborhood);
425  filterVisibleNeighbors(workState, graphNeighborhood, visibleNeighborhood);
426  //Check for addition for Coverage
427  if( !checkAddCoverage(workState, graphNeighborhood))
428  //If not for Coverage, then Connectivity
429  if( !checkAddConnectivity(workState, graphNeighborhood))
430  //Check for the existence of an interface
431  if( !checkAddInterface(graphNeighborhood, visibleNeighborhood, q))
432  {
433  // Then check to see if it's on an interface
434  getInterfaceNeighborhood(q, interfaceNeighborhood);
435  if (interfaceNeighborhood.size() > 0)
436  {
437  //Check for addition for spanner prop
438  if (!checkAddPath(q, interfaceNeighborhood))
439  //All of the tests have failed. Report failure for the sample
440  ++consecutiveFailures_;
441  }
442  else
443  //There's no interface here, so drop it
444  ++consecutiveFailures_;
445  }
446  }
447 
448  si_->freeState(workState);
449 }
450 
452 {
453  boost::mutex::scoped_lock _(graphMutex_);
454 
455  DenseVertex m = boost::add_vertex(g_);
456  stateProperty_[m] = state;
457 
458  // Which milestones will we attempt to connect to?
459  const std::vector<DenseVertex>& neighbors = connectionStrategy_(m);
460 
461  foreach (DenseVertex n, neighbors)
462  if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
463  {
464  const double weight = distanceFunction(m, n);
465  const DenseGraph::edge_property_type properties(weight);
466 
467  boost::add_edge(m, n, properties, g_);
468  }
469 
470  nn_->add(m);
471 
472  //Need to update representative information here...
473  calculateRepresentative(m);
474 
475  std::vector<DenseVertex> interfaceNeighborhood;
476  std::set<SparseVertex> interfaceRepresentatives;
477 
478  getInterfaceNeighborRepresentatives(m, interfaceRepresentatives);
479  getInterfaceNeighborhood(m, interfaceNeighborhood);
480  addToRepresentatives(m, representativesProperty_[m], interfaceRepresentatives);
481  foreach (DenseVertex qp, interfaceNeighborhood)
482  {
483  removeFromRepresentatives( qp, representativesProperty_[qp] );
484  getInterfaceNeighborRepresentatives( qp, interfaceRepresentatives );
485  addToRepresentatives( qp, representativesProperty_[qp], interfaceRepresentatives );
486  }
487 
488  return m;
489 }
490 
492 {
493  boost::mutex::scoped_lock _(graphMutex_);
494 
495  SparseVertex v = boost::add_vertex(s_);
496  sparseStateProperty_[v] = state;
497  sparseColorProperty_[v] = type;
498 
499  sparseDJSets_.make_set(v);
500 
501  snn_->add(v);
502  updateRepresentatives(v);
503 
504  resetFailures();
505  return v;
506 }
507 
509 {
510  const base::Cost weight(costHeuristic(v, vp));
511  const SpannerGraph::edge_property_type properties(weight);
512  boost::mutex::scoped_lock _(graphMutex_);
513  boost::add_edge(v, vp, properties, s_);
514  sparseDJSets_.union_set(v, vp);
515 }
516 
518 {
519  const double weight = distanceFunction(v, vp);
520  const DenseGraph::edge_property_type properties(weight);
521  boost::mutex::scoped_lock _(graphMutex_);
522  boost::add_edge(v, vp, properties, g_);
523 }
524 
525 bool ompl::geometric::SPARS::checkAddCoverage(const base::State *lastState, const std::vector<SparseVertex>& neigh )
526 {
527  //For each of these neighbors,
528  foreach (SparseVertex n, neigh)
529  //If path between is free
530  if (si_->checkMotion( lastState, sparseStateProperty_[n]))
531  //Abort out and return false
532  return false;
533  //No free paths means we add for coverage
534  addGuard(si_->cloneState(lastState), COVERAGE);
535  return true;
536 }
537 
538 bool ompl::geometric::SPARS::checkAddConnectivity( const base::State *lastState, const std::vector<SparseVertex>& neigh )
539 {
540  std::vector< SparseVertex > links;
541  //For each neighbor
542  for (std::size_t i = 0; i < neigh.size(); ++i )
543  //For each other neighbor
544  for (std::size_t j = i + 1; j < neigh.size(); ++j )
545  //If they are in different components
546  if (!sameComponent(neigh[i], neigh[j]))
547  //If the paths between are collision free
548  if( si_->checkMotion( lastState, sparseStateProperty_[neigh[i]] ) && si_->checkMotion( lastState, sparseStateProperty_[neigh[j]] ) )
549  {
550  links.push_back( neigh[i] );
551  links.push_back( neigh[j] );
552  }
553 
554  if( links.size() != 0 )
555  {
556  //Add the node
557  SparseVertex g = addGuard( si_->cloneState(lastState), CONNECTIVITY );
558 
559  for (std::size_t i = 0; i < links.size(); ++i )
560  //If there's no edge
561  if (!boost::edge(g, links[i], s_).second)
562  //And the components haven't been united by previous links
563  if (!sameComponent(links[i], g))
564  connectSparsePoints( g, links[i] );
565  return true;
566  }
567  return false;
568 }
569 
570 bool ompl::geometric::SPARS::checkAddInterface(const std::vector<SparseVertex>& graphNeighborhood, const std::vector<SparseVertex>& visibleNeighborhood, DenseVertex q )
571 {
572  //If we have more than 1 neighbor
573  if( visibleNeighborhood.size() > 1 )
574  //If our closest neighbors are also visible
575  if( graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1] )
576  //If our two closest neighbors don't share an edge
577  if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], s_).second)
578  {
579  //If they can be directly connected
580  if( si_->checkMotion( sparseStateProperty_[visibleNeighborhood[0]], sparseStateProperty_[visibleNeighborhood[1]] ) )
581  {
582  //Connect them
583  connectSparsePoints( visibleNeighborhood[0], visibleNeighborhood[1] );
584  //And report that we added to the roadmap
585  resetFailures();
586  //Report success
587  return true;
588  }
589  else
590  {
591  //Add the new node to the graph, to bridge the interface
592  SparseVertex v = addGuard( si_->cloneState( stateProperty_[q] ), INTERFACE );
593  connectSparsePoints( v, visibleNeighborhood[0] );
594  connectSparsePoints( v, visibleNeighborhood[1] );
595  //Report success
596  return true;
597  }
598  }
599  return false;
600 }
601 
602 bool ompl::geometric::SPARS::checkAddPath(DenseVertex q, const std::vector<DenseVertex>& neigh)
603 {
604  bool result = false;
605 
606  //Get q's representative => v
607  SparseVertex v = representativesProperty_[q];
608 
609  //Extract the representatives of neigh => n_rep
610  std::set<SparseVertex> n_rep;
611  foreach( DenseVertex qp, neigh )
612  n_rep.insert(representativesProperty_[qp]);
613 
614  std::vector<SparseVertex> Xs;
615  //for each v' in n_rep
616  for (std::set<SparseVertex>::iterator it = n_rep.begin() ; it != n_rep.end() && !result ; ++it)
617  {
618  SparseVertex vp = *it;
619  //Identify appropriate v" candidates => vpps
620  std::vector<SparseVertex> VPPs;
621  computeVPP(v, vp, VPPs);
622 
623  foreach( SparseVertex vpp, VPPs )
624  {
625  double s_max = 0;
626  //Find the X nodes to test
627  computeX(v, vp, vpp, Xs);
628 
629  //For each x in xs
630  foreach( SparseVertex x, Xs )
631  {
632  //Compute/Retain MAXimum distance path thorugh S
633  double dist = (si_->distance(sparseStateProperty_[x], sparseStateProperty_[v])
634  + si_->distance(sparseStateProperty_[v], sparseStateProperty_[vp])) / 2.0;
635  if( dist > s_max )
636  s_max = dist;
637  }
638 
639  DensePath bestDPath;
640  DenseVertex best_qpp = boost::graph_traits<DenseGraph>::null_vertex();
641  double d_min = std::numeric_limits<double>::infinity(); //Insanely big number
642  //For each vpp in vpps
643  for (std::size_t j = 0; j < VPPs.size() && !result; ++j)
644  {
645  SparseVertex vpp = VPPs[j];
646  //For each q", which are stored interface nodes on v for i(vpp,v)
647  foreach( DenseVertex qpp, interfaceListsProperty_[v].interfaceHash[vpp] )
648  {
649  // check that representatives are consistent
650  assert(representativesProperty_[qpp] == v);
651 
652  //If they happen to be the one and same node
653  if (q == qpp)
654  {
655  bestDPath.push_front( stateProperty_[q] );
656  best_qpp = qpp;
657  d_min = 0;
658  }
659  else
660  {
661  //Compute/Retain MINimum distance path on D through q, q"
662  DensePath dPath;
663  computeDensePath(q, qpp, dPath);
664  if (dPath.size() > 0)
665  {
666  // compute path length
667  double length = 0.0;
668  DensePath::const_iterator jt = dPath.begin();
669  for (DensePath::const_iterator it = jt + 1 ; it != dPath.end() ; ++it)
670  {
671  length += si_->distance(*jt, *it);
672  jt = it;
673  }
674 
675  if (length < d_min)
676  {
677  d_min = length;
678  bestDPath.swap(dPath);
679  best_qpp = qpp;
680  }
681  }
682  }
683  }
684 
685  //If the spanner property is violated for these paths
686  if (s_max > stretchFactor_* d_min)
687  {
688  //Need to augment this path with the appropriate neighbor information
689  DenseVertex na = getInterfaceNeighbor(q, vp);
690  DenseVertex nb = getInterfaceNeighbor(best_qpp, vpp);
691 
692  bestDPath.push_front( stateProperty_[na] );
693  bestDPath.push_back( stateProperty_[nb] );
694 
695  // check consistency of representatives
696  assert(representativesProperty_[na] == vp && representativesProperty_[nb] == vpp);
697 
698  //Add the dense path to the spanner
699  addPathToSpanner( bestDPath, vpp, vp );
700 
701  //Report success
702  result = true;
703  }
704  }
705  }
706  }
707  return result;
708 }
709 
711 {
712  double degree = 0.0;
713  foreach (DenseVertex v, boost::vertices(s_))
714  degree += (double)boost::out_degree(v, s_);
715  degree /= (double)boost::num_vertices(s_);
716  return degree;
717 }
718 
719 void ompl::geometric::SPARS::printDebug(std::ostream &out) const
720 {
721  out << "SPARS Debug Output: " << std::endl;
722  out << " Settings: " << std::endl;
723  out << " Max Failures: " << getMaxFailures() << std::endl;
724  out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
725  out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
726  out << " Stretch Factor: " << getStretchFactor() << std::endl;
727  out << " Status: " << std::endl;
728  out << " Milestone Count: " << milestoneCount() << std::endl;
729  out << " Guard Count: " << guardCount() << std::endl;
730  out << " Iterations: " << getIterationCount() << std::endl;
731  out << " Average Valence: " << averageValence() << std::endl;
732  out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
733 }
734 
735 void ompl::geometric::SPARS::getSparseNeighbors(base::State *inState, std::vector<SparseVertex> &graphNeighborhood)
736 {
737  sparseStateProperty_[sparseQueryVertex_] = inState;
738 
739  graphNeighborhood.clear();
740  snn_->nearestR(sparseQueryVertex_, sparseDelta_, graphNeighborhood);
741 
742  sparseStateProperty_[sparseQueryVertex_] = NULL;
743 }
744 
745 void ompl::geometric::SPARS::filterVisibleNeighbors(base::State *inState, const std::vector<SparseVertex> &graphNeighborhood,
746  std::vector<SparseVertex> &visibleNeighborhood) const
747 {
748  visibleNeighborhood.clear();
749  //Now that we got the neighbors from the NN, we must remove any we can't see
750  for (std::size_t i = 0; i < graphNeighborhood.size(); ++i)
751  if (si_->checkMotion(inState, sparseStateProperty_[graphNeighborhood[i]]))
752  visibleNeighborhood.push_back(graphNeighborhood[i]);
753 }
754 
756 {
757  foreach (DenseVertex vp, boost::adjacent_vertices( q, g_ ))
758  if (representativesProperty_[vp] == rep )
759  if (distanceFunction( q, vp ) <= denseDelta_)
760  return vp;
761  throw Exception(name_, "Vertex has no interface neighbor with given representative");
762 }
763 
765 {
766  // First, check to see that the path has length
767  if (dense_path.size() <= 1)
768  {
769  // The path is 0 length, so simply link the representatives
770  connectSparsePoints( vp, vpp );
771  resetFailures();
772  }
773  else
774  {
775  //We will need to construct a PathGeometric to do this.
776  geomPath_.getStates().resize( dense_path.size() );
777  std::copy( dense_path.begin(), dense_path.end(), geomPath_.getStates().begin() );
778 
779  //Attempt to simplify the path
780  psimp_->reduceVertices( geomPath_, geomPath_.getStateCount() * 2);
781 
782  // we are sure there are at least 2 points left on geomPath_
783 
784  std::vector< SparseVertex > added_nodes;
785  added_nodes.reserve(geomPath_.getStateCount());
786  for (std::size_t i = 0; i < geomPath_.getStateCount(); ++i )
787  {
788  //Add each guard
789  SparseVertex ng = addGuard( si_->cloneState(geomPath_.getState(i)), QUALITY );
790  added_nodes.push_back( ng );
791  }
792  //Link them up
793  for (std::size_t i = 1; i < added_nodes.size() ; ++i )
794  {
795  connectSparsePoints(added_nodes[i - 1], added_nodes[i]);
796  }
797  //Don't forget to link them to their representatives
798  connectSparsePoints( added_nodes[0], vp );
799  connectSparsePoints( added_nodes[added_nodes.size()-1], vpp );
800  }
801  geomPath_.getStates().clear();
802  return true;
803 }
804 
806 {
807  //Get all of the dense samples which may be affected by adding this node
808  std::vector< DenseVertex > dense_points;
809 
810  stateProperty_[ queryVertex_ ] = sparseStateProperty_[ v ];
811 
812  nn_->nearestR( queryVertex_, sparseDelta_ + denseDelta_, dense_points );
813 
814  stateProperty_[ queryVertex_ ] = NULL;
815 
816  //For each of those points
817  for (std::size_t i = 0 ; i < dense_points.size() ; ++i)
818  {
819  //Remove that point from the old representative's list(s)
820  removeFromRepresentatives( dense_points[i], representativesProperty_[dense_points[i]] );
821  //Update that point's representative
822  calculateRepresentative( dense_points[i] );
823  }
824 
825  std::set<SparseVertex> interfaceRepresentatives;
826  //For each of the points
827  for (std::size_t i = 0 ; i < dense_points.size(); ++i)
828  {
829  //Get it's representative
830  SparseVertex rep = representativesProperty_[dense_points[i]];
831  //Extract the representatives of any interface-sharing neighbors
832  getInterfaceNeighborRepresentatives( dense_points[i], interfaceRepresentatives );
833  //For sanity's sake, make sure we clear ourselves out of what this new rep might think of us
834  removeFromRepresentatives( dense_points[i], rep );
835  //Add this vertex to it's representative's list for the other representatives
836  addToRepresentatives( dense_points[i], rep, interfaceRepresentatives );
837  }
838 }
839 
841 {
842  //Get the nearest neighbors within sparseDelta_
843  std::vector<SparseVertex> graphNeighborhood;
844  getSparseNeighbors(stateProperty_[q], graphNeighborhood);
845 
846  //For each neighbor
847  for (std::size_t i = 0; i < graphNeighborhood.size(); ++i)
848  if (si_->checkMotion(stateProperty_[q], sparseStateProperty_[graphNeighborhood[i]]))
849  {
850  //update the representative
851  representativesProperty_[q] = graphNeighborhood[i];
852  //abort
853  break;
854  }
855 }
856 
857 void ompl::geometric::SPARS::addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set<SparseVertex> &oreps)
858 {
859  //If this node supports no interfaces
860  if (oreps.size() == 0)
861  {
862  //Add it to the pool of non-interface nodes
863  bool new_insert = nonInterfaceListsProperty_[rep].insert(q).second;
864 
865  // we expect this was not previously tracked
866  if (!new_insert)
867  assert(false);
868  }
869  else
870  {
871  //otherwise, for every neighbor representative
872  foreach( SparseVertex v, oreps )
873  {
874  assert(rep == representativesProperty_[q]);
875  bool new_insert = interfaceListsProperty_[rep].interfaceHash[v].insert(q).second;
876  if (!new_insert)
877  assert(false);
878  }
879  }
880 }
881 
883 {
884  // Remove the node from the non-interface points (if there)
885  nonInterfaceListsProperty_[rep].erase(q);
886 
887  // From each of the interfaces
888  foreach (SparseVertex vpp, interfaceListsProperty_[rep].interfaceHash | boost::adaptors::map_keys)
889  {
890  // Remove this node from that list
891  interfaceListsProperty_[rep].interfaceHash[vpp].erase( q );
892  }
893 }
894 
895 void ompl::geometric::SPARS::computeVPP(SparseVertex v, SparseVertex vp, std::vector<SparseVertex> &VPPs)
896 {
897  foreach( SparseVertex cvpp, boost::adjacent_vertices( v, s_ ) )
898  if( cvpp != vp )
899  if( !boost::edge( cvpp, vp, s_ ).second )
900  VPPs.push_back( cvpp );
901 }
902 
903 void ompl::geometric::SPARS::computeX(SparseVertex v, SparseVertex vp, SparseVertex vpp, std::vector<SparseVertex> &Xs)
904 {
905  Xs.clear();
906  foreach( SparseVertex cx, boost::adjacent_vertices( vpp, s_ ) )
907  if( boost::edge( cx, v, s_ ).second && !boost::edge( cx, vp, s_ ).second )
908  if (interfaceListsProperty_[vpp].interfaceHash[cx].size() > 0)
909  Xs.push_back( cx );
910  Xs.push_back( vpp );
911 }
912 
913 void ompl::geometric::SPARS::getInterfaceNeighborRepresentatives(DenseVertex q, std::set<SparseVertex> &interfaceRepresentatives)
914 {
915  interfaceRepresentatives.clear();
916 
917  // Get our representative
918  SparseVertex rep = representativesProperty_[q];
919  // For each neighbor we are connected to
920  foreach( DenseVertex n, boost::adjacent_vertices( q, g_ ) )
921  {
922  // Get his representative
923  SparseVertex orep = representativesProperty_[n];
924  // If that representative is not our own
925  if (orep != rep)
926  // If he is within denseDelta_
927  if (distanceFunction( q, n ) < denseDelta_)
928  // Include his rep in the set
929  interfaceRepresentatives.insert(orep);
930  }
931 }
932 
933 void ompl::geometric::SPARS::getInterfaceNeighborhood(DenseVertex q, std::vector<DenseVertex> &interfaceNeighborhood)
934 {
935  interfaceNeighborhood.clear();
936 
937  // Get our representative
938  SparseVertex rep = representativesProperty_[q];
939 
940  // For each neighbor we are connected to
941  foreach( DenseVertex n, boost::adjacent_vertices( q, g_ ) )
942  // If neighbor representative is not our own
943  if( representativesProperty_[n] != rep )
944  // If he is within denseDelta_
945  if( distanceFunction( q, n ) < denseDelta_ )
946  // Append him to the list
947  interfaceNeighborhood.push_back( n );
948 }
949 
951 {
952  boost::mutex::scoped_lock _(graphMutex_);
953 
954  boost::vector_property_map<SparseVertex> prev(boost::num_vertices(s_));
955 
956  try
957  {
958  // Consider using a persistent distance_map if it's slow
959  boost::astar_search(s_, start,
960  boost::bind(&SPARS::costHeuristic, this, _1, goal),
961  boost::predecessor_map(prev).
962  distance_compare(boost::bind(&base::OptimizationObjective::
963  isCostBetterThan, opt_.get(), _1, _2)).
964  distance_combine(boost::bind(&base::OptimizationObjective::
965  combineCosts, opt_.get(), _1, _2)).
966  distance_inf(opt_->infiniteCost()).
967  distance_zero(opt_->identityCost()).
968  visitor(AStarGoalVisitor<SparseVertex>(goal)));
969  }
970  catch (AStarFoundGoal&)
971  {
972  }
973 
974  if (prev[goal] == goal)
975  throw Exception(name_, "Could not find solution path");
976  else
977  {
978  PathGeometric *p = new PathGeometric(si_);
979 
980  for (SparseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
981  p->append(sparseStateProperty_[pos]);
982  p->append(sparseStateProperty_[start]);
983  p->reverse();
984 
985  return base::PathPtr(p);
986  }
987 }
988 
990 {
991  path.clear();
992 
993  boost::vector_property_map<DenseVertex> prev(boost::num_vertices(g_));
994 
995  try
996  {
997  boost::astar_search(g_, start,
998  boost::bind(&SPARS::distanceFunction, this, _1, goal),
999  boost::predecessor_map(prev).
1000  visitor(AStarGoalVisitor<DenseVertex>(goal)));
1001  }
1002  catch (AStarFoundGoal&)
1003  {
1004  }
1005 
1006  if (prev[goal] == goal)
1007  OMPL_WARN("%s: No dense path was found?", getName().c_str());
1008  else
1009  {
1010  for (DenseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
1011  path.push_front( stateProperty_[pos] );
1012  path.push_front( stateProperty_[start] );
1013  }
1014 }
1015 
1017 {
1018  Planner::getPlannerData(data);
1019 
1020  // Explicitly add start and goal states:
1021  for (std::size_t i = 0; i < startM_.size(); ++i)
1022  data.addStartVertex(base::PlannerDataVertex(sparseStateProperty_[startM_[i]], (int)START));
1023 
1024  for (std::size_t i = 0; i < goalM_.size(); ++i)
1025  data.addGoalVertex(base::PlannerDataVertex(sparseStateProperty_[goalM_[i]], (int)GOAL));
1026 
1027  // Adding edges and all other vertices simultaneously
1028  foreach (const SparseEdge e, boost::edges(s_))
1029  {
1030  const SparseVertex v1 = boost::source(e, s_);
1031  const SparseVertex v2 = boost::target(e, s_);
1032  data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v1]),
1033  base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v2]));
1034 
1035  // Add the reverse edge, since we're constructing an undirected roadmap
1036  data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v1]),
1037  base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v2]));
1038  }
1039 
1040  // Make sure to add edge-less nodes as well
1041  foreach (const SparseVertex n, boost::vertices(s_))
1042  if (boost::out_degree( n, s_ ) == 0)
1043  data.addVertex(base::PlannerDataVertex(sparseStateProperty_[n], (int)sparseColorProperty_[n]));
1044 }
1045 
1047 {
1048  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
1049 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARS.cpp:173
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARS.cpp:157
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:391
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARS.cpp:150
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true...
SPARS(const base::SpaceInformationPtr &si)
Constructor.
Definition: SPARS.cpp:54
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to S.
Definition: SPARS.h:527
The planner failed to find a solution.
Definition: PlannerStatus.h:62
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARS.cpp:101
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:208
DenseVertex getInterfaceNeighbor(DenseVertex q, SparseVertex rep)
Get the first neighbor of q who has representative rep and is within denseDelta_. ...
Definition: SPARS.cpp:755
virtual ~SPARS()
Destructor.
Definition: SPARS.cpp:96
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: SPARS.cpp:303
Abstract definition of goals.
Definition: Goal.h:63
void setDenseDeltaFraction(double d)
Set the delta fraction for interface detection. If two nodes in the dense graph are more than a delta...
Definition: SPARS.h:274
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: SPARS.cpp:1016
double distanceFunction(const DenseVertex a, const DenseVertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition: SPARS.h:464
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool checkAddPath(DenseVertex q, const std::vector< DenseVertex > &neigh)
Checks for adding an entire dense path to the Sparse Roadmap.
Definition: SPARS.cpp:602
boost::graph_traits< SpannerGraph >::edge_descriptor SparseEdge
An edge in the sparse roadmap that is constructed.
Definition: SPARS.h:160
STL namespace.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
void removeFromRepresentatives(DenseVertex q, SparseVertex rep)
Removes the node from its representative's lists.
Definition: SPARS.cpp:882
void getInterfaceNeighborRepresentatives(DenseVertex q, std::set< SparseVertex > &interfaceRepresentatives)
Gets the representatives of all interfaces that q supports.
Definition: SPARS.cpp:913
bool reachedTerminationCriterion() const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
Definition: SPARS.cpp:276
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
Definition: SPARS.cpp:390
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARS.cpp:719
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:211
void computeVPP(DenseVertex v, DenseVertex vp, std::vector< SparseVertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition: SPARS.cpp:895
unsigned int milestoneCount() const
Returns the number of milestones added to D.
Definition: SPARS.h:340
base::Cost costHeuristic(SparseVertex u, SparseVertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition: SPARS.cpp:1046
bool checkAddCoverage(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for the coverage property, and adds appropriately. ...
Definition: SPARS.cpp:525
void setMaxFailures(unsigned int m)
Set the maximum consecutive failures to augment the spanner before termination. In general...
Definition: SPARS.h:266
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta fraction.
Definition: SPARS.h:313
void calculateRepresentative(DenseVertex q)
Calculates the representative for a dense sample.
Definition: SPARS.cpp:840
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
void updateRepresentatives(SparseVertex v)
Automatically updates the representatives of all dense samplse within sparseDelta_ of v...
Definition: SPARS.cpp:805
void setStretchFactor(double t)
Set the roadmap spanner stretch factor. This value represents a multiplicative upper bound on path qu...
Definition: SPARS.h:295
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
void addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set< SparseVertex > &oreps)
Adds a dense sample to the appropriate lists of its representative.
Definition: SPARS.cpp:857
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition: SPARS.h:319
Abstract definition of a goal region that can be sampled.
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta fraction.
Definition: SPARS.h:307
DenseVertex addMilestone(base::State *state)
Construct a milestone for a given state (state) and store it in the nearest neighbors data structure...
Definition: SPARS.cpp:451
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
void getInterfaceNeighborhood(DenseVertex q, std::vector< DenseVertex > &interfaceNeighborhood)
Gets the neighbors of q who help it support an interface.
Definition: SPARS.cpp:933
boost::graph_traits< SpannerGraph >::vertex_descriptor SparseVertex
A vertex in the sparse roadmap that is constructed.
Definition: SPARS.h:157
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void connectDensePoints(DenseVertex v, DenseVertex vp)
Connects points in the dense graph.
Definition: SPARS.cpp:517
boost::graph_traits< DenseGraph >::vertex_descriptor DenseVertex
A vertex in DenseGraph.
Definition: SPARS.h:190
void computeDensePath(const DenseVertex start, const DenseVertex goal, DensePath &path) const
Constructs the dense path between the start and goal vertices (if connected)
Definition: SPARS.cpp:989
The planner found an exact solution.
Definition: PlannerStatus.h:66
void filterVisibleNeighbors(base::State *inState, const std::vector< SparseVertex > &graphNeighborhood, std::vector< SparseVertex > &visibleNeighborhood) const
Get the visible neighbors.
Definition: SPARS.cpp:745
void computeX(DenseVertex v, DenseVertex vp, DenseVertex vpp, std::vector< SparseVertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v', and v".
Definition: SPARS.cpp:903
void reverse()
Reverse the path.
bool haveSolution(const std::vector< DenseVertex > &start, const std::vector< DenseVertex > &goal, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition: SPARS.cpp:236
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
Definition: Goal.h:136
This class contains routines that attempt to simplify geometric paths.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
base::PathPtr constructSolution(const SparseVertex start, const SparseVertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition: SPARS.cpp:950
An optimization objective which corresponds to optimizing path length.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK
Maximum number of sampling attempts to find a valid state, without checking whether the allowed time ...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
unsigned getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARS.h:301
Abstract definition of optimization objectives.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
The exception type for ompl.
Definition: Exception.h:47
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: SPARS.cpp:139
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
bool checkAddInterface(const std::vector< DenseVertex > &graphNeighborhood, const std::vector< DenseVertex > &visibleNeighborhood, DenseVertex q)
Checks the latest dense sample for bridging an edge-less interface.
Definition: SPARS.cpp:570
Make the minimal number of connections required to ensure asymptotic optimality.
bool reachedFailureLimit() const
Returns true if we have reached the iteration failures limit, maxFailures_.
Definition: SPARS.cpp:281
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: SPARS.cpp:212
bool checkAddConnectivity(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for connectivity, and adds appropriately.
Definition: SPARS.cpp:538
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:218
void connectSparsePoints(SparseVertex v, SparseVertex vp)
Convenience function for creating an edge in the Spanner Roadmap.
Definition: SPARS.cpp:508
void getSparseNeighbors(base::State *inState, std::vector< SparseVertex > &graphNeighborhood)
Get all nodes in the sparse graph which are within sparseDelta_ of the given state.
Definition: SPARS.cpp:735
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
Definition: SPARS.cpp:286
Definition of a geometric path.
Definition: PathGeometric.h:60
SparseVertex addGuard(base::State *state, GuardType type)
Construct a node with the given state (state) for the spanner and store it in the nn structure...
Definition: SPARS.cpp:491
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
void resetFailures()
A reset function for resetting the failures count.
Definition: SPARS.cpp:145
double sparseDistanceFunction(const SparseVertex a, const SparseVertex b) const
Compute distance between two nodes in the sparse roadmap spanner.
Definition: SPARS.h:470
bool addPathToSpanner(const DensePath &p, SparseVertex vp, SparseVertex vpp)
Method for actually adding a dense path to the Roadmap Spanner, S.
Definition: SPARS.cpp:764
std::deque< base::State * > DensePath
Internal representation of a dense path.
Definition: SPARS.h:121
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition: SPARS.h:84
void setSparseDeltaFraction(double d)
Set the delta fraction for connection distance on the sparse spanner. This value represents the visib...
Definition: SPARS.h:285
DenseVertex addSample(base::State *workState, const base::PlannerTerminationCondition &ptc)
Attempt to add a single sample to the roadmap.
Definition: SPARS.cpp:191
bool sameComponent(SparseVertex m1, SparseVertex m2)
Check that two vertices are in the same connected component.
Definition: SPARS.cpp:298
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
A boost shared pointer wrapper for ompl::base::Path.
double averageValence() const
Returns the average valence of the spanner graph.
Definition: SPARS.cpp:710
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:55
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68