SPARStwo.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/SPARStwo.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 <boost/graph/astar_search.hpp>
43 #include <boost/graph/incremental_components.hpp>
44 #include <boost/property_map/vector_property_map.hpp>
45 #include <boost/foreach.hpp>
46 #include <thread>
47 
48 #include "GoalVisitor.hpp"
49 
50 #define foreach BOOST_FOREACH
51 #define foreach_reverse BOOST_REVERSE_FOREACH
52 
54  : base::Planner(si, "SPARStwo")
55  , stretchFactor_(3.)
56  , sparseDeltaFraction_(.25)
57  , denseDeltaFraction_(.001)
58  , maxFailures_(5000)
59  , nearSamplePoints_((2 * si_->getStateDimension()))
60  , stateProperty_(boost::get(vertex_state_t(), g_))
61  , weightProperty_(boost::get(boost::edge_weight, g_))
62  , colorProperty_(boost::get(vertex_color_t(), g_))
63  , interfaceDataProperty_(boost::get(vertex_interface_data_t(), g_))
64  , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
65  , addedSolution_(false)
66  , consecutiveFailures_(0)
67  , sparseDelta_(0.)
68  , denseDelta_(0.)
69  , iterations_(0)
70  , bestCost_(std::numeric_limits<double>::quiet_NaN())
71 {
74  specs_.optimizingPaths = true;
75  specs_.multithreaded = true;
76 
77  psimp_ = std::make_shared<PathSimplifier>(si_);
78 
79  Planner::declareParam<double>("stretch_factor", this, &SPARStwo::setStretchFactor, &SPARStwo::getStretchFactor,
80  "1.1:0.1:3.0");
81  Planner::declareParam<double>("sparse_delta_fraction", this, &SPARStwo::setSparseDeltaFraction,
82  &SPARStwo::getSparseDeltaFraction, "0.0:0.01:1.0");
83  Planner::declareParam<double>("dense_delta_fraction", this, &SPARStwo::setDenseDeltaFraction,
84  &SPARStwo::getDenseDeltaFraction, "0.0:0.0001:0.1");
85  Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures,
86  "100:10:3000");
87 
88  addPlannerProgressProperty("iterations INTEGER", [this]
89  {
90  return getIterationCount();
91  });
92  addPlannerProgressProperty("best cost REAL", [this]
93  {
94  return getBestCost();
95  });
96 }
97 
99 {
100  freeMemory();
101 }
102 
104 {
105  Planner::setup();
106  if (!nn_)
107  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
108  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
109  {
110  return distanceFunction(a, b);
111  });
112  double maxExt = si_->getMaximumExtent();
114  denseDelta_ = denseDeltaFraction_ * maxExt;
115 
116  // Setup optimization objective
117  //
118  // If no optimization objective was specified, then default to
119  // optimizing path length as computed by the distance() function
120  // in the state space.
121  if (pdef_)
122  {
123  if (pdef_->hasOptimizationObjective())
124  {
125  opt_ = pdef_->getOptimizationObjective();
126  if (!dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()))
127  OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
128  "for other optimizaton objectives is not guaranteed.",
129  getName().c_str());
130  }
131  else
132  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
133  }
134  else
135  {
136  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
137  setup_ = false;
138  }
139 }
140 
142 {
143  Planner::setProblemDefinition(pdef);
144  clearQuery();
145 }
146 
148 {
149  startM_.clear();
150  goalM_.clear();
151  pis_.restart();
152 }
153 
155 {
156  Planner::clear();
157  clearQuery();
158  resetFailures();
159  iterations_ = 0;
160  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
161  freeMemory();
162  if (nn_)
163  nn_->clear();
164 }
165 
167 {
168  Planner::clear();
169  sampler_.reset();
170 
171  foreach (Vertex v, boost::vertices(g_))
172  {
173  foreach (InterfaceData &d, interfaceDataProperty_[v] | boost::adaptors::map_values)
174  d.clear(si_);
175  if (stateProperty_[v] != nullptr)
176  si_->freeState(stateProperty_[v]);
177  stateProperty_[v] = nullptr;
178  }
179  g_.clear();
180 
181  if (nn_)
182  nn_->clear();
183 }
184 
185 bool ompl::geometric::SPARStwo::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
186  base::PathPtr &solution)
187 {
188  base::Goal *g = pdef_->getGoal().get();
189  base::Cost sol_cost(opt_->infiniteCost());
190  foreach (Vertex start, starts)
191  foreach (Vertex goal, goals)
192  {
193  // we lock because the connected components algorithm is incremental and may change disjointSets_
194  graphMutex_.lock();
195  bool same_component = sameComponent(start, goal);
196  graphMutex_.unlock();
197 
198  if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
199  {
200  base::PathPtr p = constructSolution(start, goal);
201  if (p)
202  {
203  base::Cost pathCost = p->cost(opt_);
204  if (opt_->isCostBetterThan(pathCost, bestCost_))
205  bestCost_ = pathCost;
206  // Check if optimization objective is satisfied
207  if (opt_->isSatisfied(pathCost))
208  {
209  solution = p;
210  return true;
211  }
212  else if (opt_->isCostBetterThan(pathCost, sol_cost))
213  {
214  solution = p;
215  sol_cost = pathCost;
216  }
217  }
218  }
219  }
220  return false;
221 }
222 
224 {
225  return boost::same_component(m1, m2, disjointSets_);
226 }
227 
229 {
231 }
232 
234 {
236 }
237 
239 {
240  if (stopOnMaxFail)
241  {
242  resetFailures();
243  base::PlannerTerminationCondition ptcOrFail([this, &ptc]
244  {
245  return ptc || reachedFailureLimit();
246  });
247  constructRoadmap(ptcOrFail);
248  }
249  else
250  constructRoadmap(ptc);
251 }
252 
254 {
256 
257  if (!isSetup())
258  setup();
259  if (!sampler_)
260  sampler_ = si_->allocValidStateSampler();
261 
262  base::State *qNew = si_->allocState();
263  base::State *workState = si_->allocState();
264 
265  /* The whole neighborhood set which has been most recently computed */
266  std::vector<Vertex> graphNeighborhood;
267  /* The visible neighborhood set which has been most recently computed */
268  std::vector<Vertex> visibleNeighborhood;
269 
270  bestCost_ = opt_->infiniteCost();
271  while (ptc == false)
272  {
273  ++iterations_;
275 
276  // Generate a single sample, and attempt to connect it to nearest neighbors.
277  if (!sampler_->sample(qNew))
278  continue;
279 
280  findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);
281 
282  if (!checkAddCoverage(qNew, visibleNeighborhood))
283  if (!checkAddConnectivity(qNew, visibleNeighborhood))
284  if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
285  {
286  if (visibleNeighborhood.size() > 0)
287  {
288  std::map<Vertex, base::State *> closeRepresentatives;
289  findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
290  for (auto &closeRepresentative : closeRepresentatives)
291  {
292  updatePairPoints(visibleNeighborhood[0], qNew, closeRepresentative.first,
293  closeRepresentative.second);
294  updatePairPoints(closeRepresentative.first, closeRepresentative.second,
295  visibleNeighborhood[0], qNew);
296  }
297  checkAddPath(visibleNeighborhood[0]);
298  for (auto &closeRepresentative : closeRepresentatives)
299  {
300  checkAddPath(closeRepresentative.first);
301  si_->freeState(closeRepresentative.second);
302  }
303  }
304  }
305  }
306  si_->freeState(workState);
307  si_->freeState(qNew);
308 }
309 
311 {
312  std::lock_guard<std::mutex> _(graphMutex_);
313  if (boost::num_vertices(g_) < 1)
314  {
315  queryVertex_ = boost::add_vertex(g_);
316  stateProperty_[queryVertex_] = nullptr;
317  }
318 }
319 
321 {
322  checkValidity();
324 
325  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
326 
327  if (!goal)
328  {
329  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
331  }
332 
333  // Add the valid start states as milestones
334  while (const base::State *st = pis_.nextStart())
335  startM_.push_back(addGuard(si_->cloneState(st), START));
336  if (startM_.empty())
337  {
338  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
340  }
341 
342  if (!goal->couldSample())
343  {
344  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
346  }
347 
348  // Add the valid goal states as milestones
349  while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
350  goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
351  if (goalM_.empty())
352  {
353  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
355  }
356 
357  unsigned int nrStartStates = boost::num_vertices(g_) - 1; // don't count query vertex
358  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nrStartStates);
359 
360  // Reset addedSolution_ member
361  addedSolution_ = false;
362  resetFailures();
363  base::PathPtr sol;
364  base::PlannerTerminationCondition ptcOrFail([this, &ptc]
365  {
366  return ptc || reachedFailureLimit();
367  });
368  std::thread slnThread([this, &ptcOrFail, &sol]
369  {
370  checkForSolution(ptcOrFail, sol);
371  });
372 
373  // Construct planner termination condition which also takes M into account
374  base::PlannerTerminationCondition ptcOrStop([this, &ptc]
375  {
376  return ptc || reachedTerminationCriterion();
377  });
378  constructRoadmap(ptcOrStop);
379 
380  // Ensure slnThread is ceased before exiting solve
381  slnThread.join();
382 
383  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
384 
385  if (sol)
386  pdef_->addSolutionPath(sol, false, -1.0, getName());
387 
388  // Return true if any solution was found.
390 }
391 
393 {
394  base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
395  while (!ptc && !addedSolution_)
396  {
397  // Check for any new goal states
398  if (goal->maxSampleCount() > goalM_.size())
399  {
400  const base::State *st = pis_.nextGoal();
401  if (st)
402  goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
403  }
404 
405  // Check for a solution
407  // Sleep for 1ms
408  if (!addedSolution_)
409  std::this_thread::sleep_for(std::chrono::milliseconds(1));
410  }
411 }
412 
413 bool ompl::geometric::SPARStwo::checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
414 {
415  if (visibleNeighborhood.size() > 0)
416  return false;
417  // No free paths means we add for coverage
418  addGuard(si_->cloneState(qNew), COVERAGE);
419  return true;
420 }
421 
422 bool ompl::geometric::SPARStwo::checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
423 {
424  std::vector<Vertex> links;
425  if (visibleNeighborhood.size() > 1)
426  {
427  // For each neighbor
428  for (std::size_t i = 0; i < visibleNeighborhood.size(); ++i)
429  // For each other neighbor
430  for (std::size_t j = i + 1; j < visibleNeighborhood.size(); ++j)
431  // If they are in different components
432  if (!sameComponent(visibleNeighborhood[i], visibleNeighborhood[j]))
433  {
434  links.push_back(visibleNeighborhood[i]);
435  links.push_back(visibleNeighborhood[j]);
436  }
437 
438  if (links.size() > 0)
439  {
440  // Add the node
441  Vertex g = addGuard(si_->cloneState(qNew), CONNECTIVITY);
442 
443  for (unsigned long link : links)
444  // If there's no edge
445  if (!boost::edge(g, link, g_).second)
446  // And the components haven't been united by previous links
447  if (!sameComponent(link, g))
448  connectGuards(g, link);
449  return true;
450  }
451  }
452  return false;
453 }
454 
455 bool ompl::geometric::SPARStwo::checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
456  std::vector<Vertex> &visibleNeighborhood)
457 {
458  // If we have more than 1 or 0 neighbors
459  if (visibleNeighborhood.size() > 1)
460  if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
461  // If our two closest neighbors don't share an edge
462  if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], g_).second)
463  {
464  // If they can be directly connected
465  if (si_->checkMotion(stateProperty_[visibleNeighborhood[0]], stateProperty_[visibleNeighborhood[1]]))
466  {
467  // Connect them
468  connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]);
469  // And report that we added to the roadmap
470  resetFailures();
471  // Report success
472  return true;
473  }
474  else
475  {
476  // Add the new node to the graph, to bridge the interface
477  Vertex v = addGuard(si_->cloneState(qNew), INTERFACE);
478  connectGuards(v, visibleNeighborhood[0]);
479  connectGuards(v, visibleNeighborhood[1]);
480  // Report success
481  return true;
482  }
483  }
484  return false;
485 }
486 
488 {
489  bool ret = false;
490 
491  std::vector<Vertex> rs;
492  foreach (Vertex r, boost::adjacent_vertices(v, g_))
493  rs.push_back(r);
494 
495  /* Candidate x vertices as described in the method, filled by function computeX(). */
496  std::vector<Vertex> Xs;
497 
498  /* Candidate v" vertices as described in the method, filled by function computeVPP(). */
499  std::vector<Vertex> VPPs;
500 
501  for (std::size_t i = 0; i < rs.size() && !ret; ++i)
502  {
503  Vertex r = rs[i];
504  computeVPP(v, r, VPPs);
505  foreach (Vertex rp, VPPs)
506  {
507  // First, compute the longest path through the graph
508  computeX(v, r, rp, Xs);
509  double rm_dist = 0.0;
510  foreach (Vertex rpp, Xs)
511  {
512  double tmp_dist = (si_->distance(stateProperty_[r], stateProperty_[v]) +
513  si_->distance(stateProperty_[v], stateProperty_[rpp])) /
514  2.0;
515  if (tmp_dist > rm_dist)
516  rm_dist = tmp_dist;
517  }
518 
519  InterfaceData &d = getData(v, r, rp);
520 
521  // Then, if the spanner property is violated
522  if (rm_dist > stretchFactor_ * d.d_)
523  {
524  ret = true; // Report that we added for the path
525  if (si_->checkMotion(stateProperty_[r], stateProperty_[rp]))
526  connectGuards(r, rp);
527  else
528  {
529  auto p(std::make_shared<PathGeometric>(si_));
530  if (r < rp)
531  {
532  p->append(d.sigmaA_);
533  p->append(d.pointA_);
534  p->append(stateProperty_[v]);
535  p->append(d.pointB_);
536  p->append(d.sigmaB_);
537  }
538  else
539  {
540  p->append(d.sigmaB_);
541  p->append(d.pointB_);
542  p->append(stateProperty_[v]);
543  p->append(d.pointA_);
544  p->append(d.sigmaA_);
545  }
546 
547  psimp_->reduceVertices(*p, 10);
548  psimp_->shortcutPath(*p, 50);
549 
550  if (p->checkAndRepair(100).second)
551  {
552  Vertex prior = r;
553  Vertex vnew;
554  std::vector<base::State *> &states = p->getStates();
555 
556  foreach (base::State *st, states)
557  {
558  // no need to clone st, since we will destroy p; we just copy the pointer
559  vnew = addGuard(st, QUALITY);
560 
561  connectGuards(prior, vnew);
562  prior = vnew;
563  }
564  // clear the states, so memory is not freed twice
565  states.clear();
566  connectGuards(prior, rp);
567  }
568  }
569  }
570  }
571  }
572 
573  return ret;
574 }
575 
577 {
579 }
580 
581 void ompl::geometric::SPARStwo::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood,
582  std::vector<Vertex> &visibleNeighborhood)
583 {
584  visibleNeighborhood.clear();
586  nn_->nearestR(queryVertex_, sparseDelta_, graphNeighborhood);
587  stateProperty_[queryVertex_] = nullptr;
588 
589  // Now that we got the neighbors from the NN, we must remove any we can't see
590  for (unsigned long i : graphNeighborhood)
591  if (si_->checkMotion(st, stateProperty_[i]))
592  visibleNeighborhood.push_back(i);
593 }
594 
596 {
597  std::vector<Vertex> hold;
598  nn_->nearestR(v, sparseDelta_, hold);
599 
600  std::vector<Vertex> neigh;
601  for (unsigned long i : hold)
602  if (si_->checkMotion(stateProperty_[v], stateProperty_[i]))
603  neigh.push_back(i);
604 
605  foreach (Vertex vp, neigh)
606  connectGuards(v, vp);
607 }
608 
610 {
611  std::vector<Vertex> nbh;
613  nn_->nearestR(queryVertex_, sparseDelta_, nbh);
614  stateProperty_[queryVertex_] = nullptr;
615 
616  Vertex result = boost::graph_traits<Graph>::null_vertex();
617 
618  for (unsigned long i : nbh)
619  if (si_->checkMotion(st, stateProperty_[i]))
620  {
621  result = i;
622  break;
623  }
624  return result;
625 }
626 
628  const Vertex qRep,
629  std::map<Vertex, base::State *> &closeRepresentatives,
631 {
632  for (auto &closeRepresentative : closeRepresentatives)
633  si_->freeState(closeRepresentative.second);
634  closeRepresentatives.clear();
635 
636  // Then, begin searching the space around him
637  for (unsigned int i = 0; i < nearSamplePoints_; ++i)
638  {
639  do
640  {
641  sampler_->sampleNear(workArea, qNew, denseDelta_);
642  } while ((!si_->isValid(workArea) || si_->distance(qNew, workArea) > denseDelta_ ||
643  !si_->checkMotion(qNew, workArea)) &&
644  ptc == false);
645 
646  // if we were not successful at sampling a desirable state, we are out of time
647  if (ptc == true)
648  break;
649 
650  // Compute who his graph neighbors are
651  Vertex representative = findGraphRepresentative(workArea);
652 
653  // Assuming this sample is actually seen by somebody (which he should be in all likelihood)
654  if (representative != boost::graph_traits<Graph>::null_vertex())
655  {
656  // If his representative is different than qNew
657  if (qRep != representative)
658  // And we haven't already tracked this representative
659  if (closeRepresentatives.find(representative) == closeRepresentatives.end())
660  // Track the representative
661  closeRepresentatives[representative] = si_->cloneState(workArea);
662  }
663  else
664  {
665  // This guy can't be seen by anybody, so we should take this opportunity to add him
666  addGuard(si_->cloneState(workArea), COVERAGE);
667 
668  // We should also stop our efforts to add a dense path
669  for (auto &closeRepresentative : closeRepresentatives)
670  si_->freeState(closeRepresentative.second);
671  closeRepresentatives.clear();
672  break;
673  }
674  }
675 }
676 
678 {
679  // First of all, we need to compute all candidate r'
680  std::vector<Vertex> VPPs;
681  computeVPP(rep, r, VPPs);
682 
683  // Then, for each pair Pv(r,r')
684  foreach (Vertex rp, VPPs)
685  // Try updating the pair info
686  distanceCheck(rep, q, r, s, rp);
687 }
688 
689 void ompl::geometric::SPARStwo::computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs)
690 {
691  VPPs.clear();
692  foreach (Vertex cvpp, boost::adjacent_vertices(v, g_))
693  if (cvpp != vp)
694  if (!boost::edge(cvpp, vp, g_).second)
695  VPPs.push_back(cvpp);
696 }
697 
698 void ompl::geometric::SPARStwo::computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs)
699 {
700  Xs.clear();
701 
702  foreach (Vertex cx, boost::adjacent_vertices(vpp, g_))
703  if (boost::edge(cx, v, g_).second && !boost::edge(cx, vp, g_).second)
704  {
705  InterfaceData &d = getData(v, vpp, cx);
706  if ((vpp < cx && d.pointA_) || (cx < vpp && d.pointB_))
707  Xs.push_back(cx);
708  }
709  Xs.push_back(vpp);
710 }
711 
713 {
714  if (vp < vpp)
715  return VertexPair(vp, vpp);
716  else if (vpp < vp)
717  return VertexPair(vpp, vp);
718  else
719  throw Exception(name_, "Trying to get an index where the pairs are the same point!");
720 }
721 
723 {
724  return interfaceDataProperty_[v][index(vp, vpp)];
725 }
726 
728  Vertex rp)
729 {
730  // Get the info for the current representative-neighbors pair
731  InterfaceData &d = getData(rep, r, rp);
732 
733  if (r < rp) // FIRST points represent r (the guy discovered through sampling)
734  {
735  if (d.pointA_ == nullptr) // If the point we're considering replacing (P_v(r,.)) isn't there
736  // Then we know we're doing better, so add it
737  d.setFirst(q, s, si_);
738  else // Otherwise, he is there,
739  {
740  if (d.pointB_ == nullptr) // But if the other guy doesn't exist, we can't compare.
741  {
742  // Should probably keep the one that is further away from rep? Not known what to do in this case.
743  // \todo: is this not part of the algorithm?
744  }
745  else // We know both of these points exist, so we can check some distances
746  if (si_->distance(q, d.pointB_) < si_->distance(d.pointA_, d.pointB_))
747  // Distance with the new point is good, so set it.
748  d.setFirst(q, s, si_);
749  }
750  }
751  else // SECOND points represent r (the guy discovered through sampling)
752  {
753  if (d.pointB_ == nullptr) // If the point we're considering replacing (P_V(.,r)) isn't there...
754  // Then we must be doing better, so add it
755  d.setSecond(q, s, si_);
756  else // Otherwise, he is there
757  {
758  if (d.pointA_ == nullptr) // But if the other guy doesn't exist, we can't compare.
759  {
760  // Should we be doing something cool here?
761  }
762  else if (si_->distance(q, d.pointA_) < si_->distance(d.pointB_, d.pointA_))
763  // Distance with the new point is good, so set it
764  d.setSecond(q, s, si_);
765  }
766  }
767 
768  // Lastly, save what we have discovered
769  interfaceDataProperty_[rep][index(r, rp)] = d;
770 }
771 
773 {
775 
776  std::vector<Vertex> hold;
777  nn_->nearestR(queryVertex_, sparseDelta_, hold);
778 
779  stateProperty_[queryVertex_] = nullptr;
780 
781  // For each of the vertices
782  foreach (Vertex v, hold)
783  {
784  foreach (VertexPair r, interfaceDataProperty_[v] | boost::adaptors::map_keys)
785  interfaceDataProperty_[v][r].clear(si_);
786  }
787 }
788 
790 {
791  std::lock_guard<std::mutex> _(graphMutex_);
792 
793  Vertex m = boost::add_vertex(g_);
794  stateProperty_[m] = state;
795  colorProperty_[m] = type;
796 
797  assert(si_->isValid(state));
798  abandonLists(state);
799 
800  disjointSets_.make_set(m);
801  nn_->add(m);
802  resetFailures();
803 
804  return m;
805 }
806 
808 {
809  assert(v <= milestoneCount());
810  assert(vp <= milestoneCount());
811 
812  const base::Cost weight(costHeuristic(v, vp));
813  const Graph::edge_property_type properties(weight);
814  std::lock_guard<std::mutex> _(graphMutex_);
815  boost::add_edge(v, vp, properties, g_);
816  disjointSets_.union_set(v, vp);
817 }
818 
820 {
821  std::lock_guard<std::mutex> _(graphMutex_);
822 
823  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
824 
825  try
826  {
827  boost::astar_search(g_, start,
828  [this, goal](Vertex v)
829  {
830  return costHeuristic(v, goal);
831  },
832  boost::predecessor_map(prev)
833  .distance_compare([this](base::Cost c1, base::Cost c2)
834  {
835  return opt_->isCostBetterThan(c1, c2);
836  })
837  .distance_combine([this](base::Cost c1, base::Cost c2)
838  {
839  return opt_->combineCosts(c1, c2);
840  })
841  .distance_inf(opt_->infiniteCost())
842  .distance_zero(opt_->identityCost())
843  .visitor(AStarGoalVisitor<Vertex>(goal)));
844  }
845  catch (AStarFoundGoal &)
846  {
847  }
848 
849  if (prev[goal] == goal)
850  throw Exception(name_, "Could not find solution path");
851  else
852  {
853  auto p(std::make_shared<PathGeometric>(si_));
854  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
855  p->append(stateProperty_[pos]);
856  p->append(stateProperty_[start]);
857  p->reverse();
858 
859  return p;
860  }
861 }
862 
863 void ompl::geometric::SPARStwo::printDebug(std::ostream &out) const
864 {
865  out << "SPARStwo Debug Output: " << std::endl;
866  out << " Settings: " << std::endl;
867  out << " Max Failures: " << getMaxFailures() << std::endl;
868  out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
869  out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
870  out << " Stretch Factor: " << getStretchFactor() << std::endl;
871  out << " Status: " << std::endl;
872  out << " Milestone Count: " << milestoneCount() << std::endl;
873  out << " Iterations: " << getIterationCount() << std::endl;
874  out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
875 }
876 
878 {
879  Planner::getPlannerData(data);
880 
881  // Explicitly add start and goal states:
882  for (unsigned long i : startM_)
884 
885  for (unsigned long i : goalM_)
887 
888  // If there are even edges here
889  if (boost::num_edges(g_) > 0)
890  {
891  // Adding edges and all other vertices simultaneously
892  foreach (const Edge e, boost::edges(g_))
893  {
894  const Vertex v1 = boost::source(e, g_);
895  const Vertex v2 = boost::target(e, g_);
898 
899  // Add the reverse edge, since we're constructing an undirected roadmap
902  }
903  }
904  else
905  OMPL_INFORM("%s: There are no edges in the graph!", getName().c_str());
906 
907  // Make sure to add edge-less nodes as well
908  foreach (const Vertex n, boost::vertices(g_))
909  if (boost::out_degree(n, g_) == 0)
911 }
912 
914 {
915  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
916 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARStwo.cpp:863
bool sameComponent(Vertex m1, Vertex m2)
Check if two milestones (m1 and m2) are part of the same connected component. This is not a const fun...
Definition: SPARStwo.cpp:223
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
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition: SPARStwo.h:482
Graph g_
Connectivity graph.
Definition: SPARStwo.h:467
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Definition: SPARStwo.h:101
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
void findCloseRepresentatives(base::State *workArea, const base::State *qNew, Vertex qRep, std::map< Vertex, base::State *> &closeRepresentatives, const base::PlannerTerminationCondition &ptc)
Finds representatives of samples near qNew_ which are not his representative.
Definition: SPARStwo.cpp:627
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: SPARStwo.h:461
bool haveSolution(const std::vector< Vertex > &start, const std::vector< Vertex > &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: SPARStwo.cpp:185
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: SPARStwo.cpp:141
A shared pointer wrapper for ompl::base::ProblemDefinition.
base::PathPtr constructSolution(const Vertex start, const Vertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition: SPARStwo.cpp:819
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition: SPARStwo.h:252
The planner failed to find a solution.
Definition: PlannerStatus.h:62
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
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.
void findGraphNeighbors(base::State *st, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near st.
Definition: SPARStwo.cpp:581
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent...
Definition: SPARStwo.h:486
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition: SPARStwo.h:540
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARStwo.cpp:154
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
void setStretchFactor(double t)
Sets the stretch factor.
Definition: SPARStwo.h:238
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
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...
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition: SPARStwo.h:272
void resetFailures()
A reset function for resetting the failures count.
Definition: SPARStwo.cpp:576
void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp)
Performs distance checking for the candidate new state, q against the current information.
Definition: SPARStwo.cpp:727
Abstract definition of goals.
Definition: Goal.h:62
SPARStwo(const base::SpaceInformationPtr &si)
Constructor.
Definition: SPARStwo.cpp:53
base::Cost costHeuristic(Vertex u, Vertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition: SPARStwo.cpp:913
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st.
Definition: SPARStwo.cpp:609
STL namespace.
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: SPARStwo.h:542
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
std::vector< Vertex > startM_
Array of start milestones.
Definition: SPARStwo.h:470
double getStretchFactor() const
Retrieve the spanner&#39;s set stretch factor.
Definition: SPARStwo.h:284
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARStwo.cpp:147
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: SPARStwo.h:473
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition: SPARStwo.h:244
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
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARStwo.cpp:103
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition: SPARStwo.h:531
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
Definition: SPARStwo.h:105
bool checkAddCoverage(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure coverage of the space.
Definition: SPARStwo.cpp:413
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition: SPARStwo.h:519
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:438
unsigned int milestoneCount() const
Get the number of vertices in the sparse roadmap.
Definition: SPARStwo.h:340
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition: SPARStwo.h:525
Abstract definition of a goal region that can be sampled.
bool checkAddConnectivity(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure connectivity.
Definition: SPARStwo.cpp:422
void computeVPP(Vertex v, Vertex vp, std::vector< Vertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition: SPARStwo.cpp:689
bool addedSolution_
A flag indicating that a solution has been added during solve()
Definition: SPARStwo.h:516
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the second interface (i.e. interface with larger index vertex).
Definition: SPARStwo.h:163
Vertex addGuard(base::State *state, GuardType type)
Construct a guard for a given state (state) and store it in the nearest neighbors data structure...
Definition: SPARStwo.cpp:789
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
Definition: SPARStwo.cpp:253
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
The planner found an exact solution.
Definition: PlannerStatus.h:66
Interface information storage class, which does bookkeeping for criterion four.
Definition: SPARStwo.h:98
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition: SPARStwo.h:522
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: SPARStwo.cpp:320
bool checkAddInterface(const base::State *qNew, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the current sample reveals the existence of an interface, and if so...
Definition: SPARStwo.cpp:455
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition: SPARStwo.cpp:595
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...
boost::disjoint_sets< boost::property_map< Graph, boost::vertex_rank_t >::type, boost::property_map< Graph, boost::vertex_predecessor_t >::type > disjointSets_
Data structure that maintains the connected components.
Definition: SPARStwo.h:511
A shared pointer wrapper for ompl::base::SpaceInformation.
bool reachedTerminationCriterion() const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
Definition: SPARStwo.cpp:233
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition: SPARStwo.h:122
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
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
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition: SPARStwo.h:498
#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::mutex graphMutex_
Mutex to guard access to the Graph member (g_)
Definition: SPARStwo.h:528
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition: SPARStwo.h:260
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition: SPARStwo.h:489
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: SPARStwo.cpp:877
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
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition: SPARStwo.h:227
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition: SPARStwo.h:455
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition: SPARStwo.cpp:722
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition: SPARStwo.h:81
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition: SPARStwo.cpp:807
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition: SPARStwo.h:504
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: SPARStwo.cpp:392
bool reachedFailureLimit() const
Returns whether we have reached the iteration failures limit, maxFailures_.
Definition: SPARStwo.cpp:228
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition: SPARStwo.h:224
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
std::string name_
The name of this planner.
Definition: Planner.h:424
~SPARStwo() override
Destructor.
Definition: SPARStwo.cpp:98
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:216
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition: SPARStwo.h:95
void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)
High-level method which updates pair point information for repV_ with neighbor r. ...
Definition: SPARStwo.cpp:677
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:171
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
Definition: SPARStwo.cpp:310
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition: SPARStwo.h:109
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality) ...
Definition: SPARStwo.h:479
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition: SPARStwo.cpp:487
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARStwo.cpp:166
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
void abandonLists(base::State *st)
When a new guard is added at state st, finds all guards who must abandon their interface information ...
Definition: SPARStwo.cpp:772
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition: SPARStwo.h:278
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: SPARStwo.h:495
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition: SPARStwo.cpp:712
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARStwo.h:266
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition: SPARStwo.h:476
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition: SPARStwo.h:507
void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector< Vertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v&#39;, and v".
Definition: SPARStwo.cpp:698
void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the first interface (i.e. interface with smaller index vertex).
Definition: SPARStwo.h:148
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition: SPARStwo.h:464
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition: SPARStwo.h:492
A shared pointer wrapper for ompl::base::Path.
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