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