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