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