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