PRM.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Rice University
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 Rice 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: Ioan Sucan, James D. Marble, Ryan Luna */
36 
37 #include "ompl/geometric/planners/prm/PRM.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/datastructures/PDF.h"
42 #include "ompl/tools/config/SelfConfig.h"
43 #include "ompl/tools/config/MagicConstants.h"
44 #include <boost/graph/astar_search.hpp>
45 #include <boost/graph/incremental_components.hpp>
46 #include <boost/property_map/vector_property_map.hpp>
47 #include <boost/foreach.hpp>
48 #include <thread>
49 #include <typeinfo>
50 
51 #include "GoalVisitor.hpp"
52 
53 #define foreach BOOST_FOREACH
54 
55 namespace ompl
56 {
57  namespace magic
58  {
61  static const unsigned int MAX_RANDOM_BOUNCE_STEPS = 5;
62 
64  static const double ROADMAP_BUILD_TIME = 0.2;
65 
68  static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
69  } // namespace magic
70 } // namespace ompl
71 
72 ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy)
73  : base::Planner(si, "PRM")
74  , starStrategy_(starStrategy)
75  , stateProperty_(boost::get(vertex_state_t(), g_))
76  , totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_))
77  , successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_))
78  , weightProperty_(boost::get(boost::edge_weight, g_))
79  , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
80 {
83  specs_.optimizingPaths = true;
84  specs_.multithreaded = true;
85 
86  if (!starStrategy_)
87  Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors,
88  &PRM::getMaxNearestNeighbors, std::string("8:1000"));
89 
90  addPlannerProgressProperty("iterations INTEGER", [this] { return getIterationCount(); });
91  addPlannerProgressProperty("best cost REAL", [this] { return getBestCost(); });
92  addPlannerProgressProperty("milestone count INTEGER", [this] { return getMilestoneCountString(); });
93  addPlannerProgressProperty("edge count INTEGER", [this] { return getEdgeCountString(); });
94 }
95 
96 ompl::geometric::PRM::~PRM()
97 {
98  freeMemory();
99 }
100 
102 {
103  Planner::setup();
104  if (!nn_)
105  {
106  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
107  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
108  specs_.multithreaded = true;
109  nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
110  }
111  if (!connectionStrategy_)
112  setDefaultConnectionStrategy();
113  if (!connectionFilter_)
114  connectionFilter_ = [](const Vertex &, const Vertex &) { return true; };
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  opt_ = pdef_->getOptimizationObjective();
125  else
126  {
127  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
128  if (!starStrategy_)
129  opt_->setCostThreshold(opt_->infiniteCost());
130  }
131  }
132  else
133  {
134  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
135  setup_ = false;
136  }
137 }
138 
140 {
141  if (starStrategy_)
142  throw Exception("Cannot set the maximum nearest neighbors for " + getName());
143  if (!nn_)
144  {
145  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
146  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
147  specs_.multithreaded = true;
148  nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
149  }
150  if (!userSetConnectionStrategy_)
151  connectionStrategy_ = KStrategy<Vertex>(k, nn_);
152  if (isSetup())
153  setup();
154 }
155 
157 {
158  const auto strategy = connectionStrategy_.target<KStrategy<Vertex>>();
159  return strategy ? strategy->getNumNeighbors() : 0u;
160 }
161 
163 {
164  if (starStrategy_)
165  connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
166  else
167  connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
168 }
169 
170 void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
171 {
172  Planner::setProblemDefinition(pdef);
173  clearQuery();
174 }
175 
177 {
178  startM_.clear();
179  goalM_.clear();
180  pis_.restart();
181 }
182 
184 {
185  Planner::clear();
186  sampler_.reset();
187  simpleSampler_.reset();
188  freeMemory();
189  if (nn_)
190  nn_->clear();
191  clearQuery();
192 
193  iterations_ = 0;
194  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
195 }
196 
198 {
199  foreach (Vertex v, boost::vertices(g_))
200  si_->freeState(stateProperty_[v]);
201  g_.clear();
202 }
203 
204 void ompl::geometric::PRM::expandRoadmap(double expandTime)
205 {
206  expandRoadmap(base::timedPlannerTerminationCondition(expandTime));
207 }
208 
210 {
211  if (!simpleSampler_)
212  simpleSampler_ = si_->allocStateSampler();
213 
214  std::vector<base::State *> states(magic::MAX_RANDOM_BOUNCE_STEPS);
215  si_->allocStates(states);
216  expandRoadmap(ptc, states);
217  si_->freeStates(states);
218 }
219 
221  std::vector<base::State *> &workStates)
222 {
223  // construct a probability distribution over the vertices in the roadmap
224  // as indicated in
225  // "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
226  // Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
227 
228  PDF<Vertex> pdf;
229  foreach (Vertex v, boost::vertices(g_))
230  {
231  const unsigned long int t = totalConnectionAttemptsProperty_[v];
232  pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
233  }
234 
235  if (pdf.empty())
236  return;
237 
238  while (!ptc)
239  {
240  iterations_++;
241  Vertex v = pdf.sample(rng_.uniform01());
242  unsigned int s =
243  si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
244  if (s > 0)
245  {
246  s--;
247  Vertex last = addMilestone(si_->cloneState(workStates[s]));
248 
249  graphMutex_.lock();
250  for (unsigned int i = 0; i < s; ++i)
251  {
252  // add the vertex along the bouncing motion
253  Vertex m = boost::add_vertex(g_);
254  stateProperty_[m] = si_->cloneState(workStates[i]);
255  totalConnectionAttemptsProperty_[m] = 1;
256  successfulConnectionAttemptsProperty_[m] = 0;
257  disjointSets_.make_set(m);
258 
259  // add the edge to the parent vertex
260  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
261  const Graph::edge_property_type properties(weight);
262  boost::add_edge(v, m, properties, g_);
263  uniteComponents(v, m);
264 
265  // add the vertex to the nearest neighbors data structure
266  nn_->add(m);
267  v = m;
268  }
269 
270  // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
271  // we add an edge
272  if (s > 0 || !sameComponent(v, last))
273  {
274  // add the edge to the parent vertex
275  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
276  const Graph::edge_property_type properties(weight);
277  boost::add_edge(v, last, properties, g_);
278  uniteComponents(v, last);
279  }
280  graphMutex_.unlock();
281  }
282  }
283 }
284 
285 void ompl::geometric::PRM::growRoadmap(double growTime)
286 {
287  growRoadmap(base::timedPlannerTerminationCondition(growTime));
288 }
289 
291 {
292  if (!isSetup())
293  setup();
294  if (!sampler_)
295  sampler_ = si_->allocValidStateSampler();
296 
297  base::State *workState = si_->allocState();
298  growRoadmap(ptc, workState);
299  si_->freeState(workState);
300 }
301 
303 {
304  /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
305  while (!ptc)
306  {
307  iterations_++;
308  // search for a valid state
309  bool found = false;
310  while (!found && !ptc)
311  {
312  unsigned int attempts = 0;
313  do
314  {
315  found = sampler_->sample(workState);
316  attempts++;
317  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
318  }
319  // add it as a milestone
320  if (found)
321  addMilestone(si_->cloneState(workState));
322  }
323 }
324 
325 void ompl::geometric::PRM::checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
326 {
327  auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
328  while (!ptc && !addedNewSolution_)
329  {
330  // Check for any new goal states
331  if (goal->maxSampleCount() > goalM_.size())
332  {
333  const base::State *st = pis_.nextGoal();
334  if (st != nullptr)
335  goalM_.push_back(addMilestone(si_->cloneState(st)));
336  }
337 
338  // Check for a solution
339  addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution);
340  // Sleep for 1ms
341  if (!addedNewSolution_)
342  std::this_thread::sleep_for(std::chrono::milliseconds(1));
343  }
344 }
345 
346 bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
347  base::PathPtr &solution)
348 {
349  base::Goal *g = pdef_->getGoal().get();
350  base::Cost sol_cost(opt_->infiniteCost());
351  foreach (Vertex start, starts)
352  {
353  foreach (Vertex goal, goals)
354  {
355  // we lock because the connected components algorithm is incremental and may change disjointSets_
356  graphMutex_.lock();
357  bool same_component = sameComponent(start, goal);
358  graphMutex_.unlock();
359 
360  if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
361  {
362  base::PathPtr p = constructSolution(start, goal);
363  if (p)
364  {
365  base::Cost pathCost = p->cost(opt_);
366  if (opt_->isCostBetterThan(pathCost, bestCost_))
367  bestCost_ = pathCost;
368  // Check if optimization objective is satisfied
369  if (opt_->isSatisfied(pathCost))
370  {
371  solution = p;
372  return true;
373  }
374  if (opt_->isCostBetterThan(pathCost, sol_cost))
375  {
376  solution = p;
377  sol_cost = pathCost;
378  }
379  }
380  }
381  }
382  }
383 
384  return false;
385 }
386 
388 {
389  return addedNewSolution_;
390 }
391 
393 {
394  checkValidity();
395  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
396 
397  if (goal == nullptr)
398  {
399  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
401  }
402 
403  // Add the valid start states as milestones
404  while (const base::State *st = pis_.nextStart())
405  startM_.push_back(addMilestone(si_->cloneState(st)));
406 
407  if (startM_.empty())
408  {
409  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
411  }
412 
413  if (!goal->couldSample())
414  {
415  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
417  }
418 
419  // Ensure there is at least one valid goal state
420  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
421  {
422  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
423  if (st != nullptr)
424  goalM_.push_back(addMilestone(si_->cloneState(st)));
425 
426  if (goalM_.empty())
427  {
428  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
430  }
431  }
432 
433  unsigned long int nrStartStates = boost::num_vertices(g_);
434  OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
435 
436  // Reset addedNewSolution_ member and create solution checking thread
437  addedNewSolution_ = false;
438  base::PathPtr sol;
439  std::thread slnThread([this, &ptc, &sol] { checkForSolution(ptc, sol); });
440 
441  // construct new planner termination condition that fires when the given ptc is true, or a solution is found
442  base::PlannerTerminationCondition ptcOrSolutionFound([this, &ptc] { return ptc || addedNewSolution(); });
443 
444  constructRoadmap(ptcOrSolutionFound);
445 
446  // Ensure slnThread is ceased before exiting solve
447  slnThread.join();
448 
449  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
450 
451  if (sol)
452  {
453  base::PlannerSolution psol(sol);
454  psol.setPlannerName(getName());
455  // if the solution was optimized, we mark it as such
456  psol.setOptimized(opt_, bestCost_, addedNewSolution());
457  pdef_->addSolutionPath(psol);
458  }
459  else
460  {
461  // Return an approximate solution.
462  ompl::base::Cost diff = constructApproximateSolution(startM_, goalM_, sol);
463  if (!opt_->isFinite(diff))
464  {
465  OMPL_INFORM("Closest path is still start and goal");
467  }
468  OMPL_INFORM("Using approximate solution, heuristic cost-to-go is %f", diff.value());
469  pdef_->addSolutionPath(sol, true, diff.value(), getName());
471  }
472 
474 }
475 
477 {
478  if (!isSetup())
479  setup();
480  if (!sampler_)
481  sampler_ = si_->allocValidStateSampler();
482  if (!simpleSampler_)
483  simpleSampler_ = si_->allocStateSampler();
484 
485  std::vector<base::State *> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
486  si_->allocStates(xstates);
487  bool grow = true;
488 
489  bestCost_ = opt_->infiniteCost();
490  while (!ptc())
491  {
492  // maintain a 2:1 ratio for growing/expansion of roadmap
493  // call growRoadmap() twice as long for every call of expandRoadmap()
494  if (grow)
497  xstates[0]);
498  else
501  xstates);
502  grow = !grow;
503  }
504 
505  si_->freeStates(xstates);
506 }
507 
509 {
510  std::lock_guard<std::mutex> _(graphMutex_);
511 
512  Vertex m = boost::add_vertex(g_);
513  stateProperty_[m] = state;
514  totalConnectionAttemptsProperty_[m] = 1;
515  successfulConnectionAttemptsProperty_[m] = 0;
516 
517  // Initialize to its own (dis)connected component.
518  disjointSets_.make_set(m);
519 
520  // Which milestones will we attempt to connect to?
521  const std::vector<Vertex> &neighbors = connectionStrategy_(m);
522 
523  foreach (Vertex n, neighbors)
524  if (connectionFilter_(n, m))
525  {
526  totalConnectionAttemptsProperty_[m]++;
527  totalConnectionAttemptsProperty_[n]++;
528  if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
529  {
530  successfulConnectionAttemptsProperty_[m]++;
531  successfulConnectionAttemptsProperty_[n]++;
532  const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
533  const Graph::edge_property_type properties(weight);
534  boost::add_edge(n, m, properties, g_);
535  uniteComponents(n, m);
536  }
537  }
538 
539  nn_->add(m);
540 
541  return m;
542 }
543 
545 {
546  disjointSets_.union_set(m1, m2);
547 }
548 
550 {
551  return boost::same_component(m1, m2, disjointSets_);
552 }
553 
555  const std::vector<Vertex> &goals,
556  base::PathPtr &solution)
557 {
558  std::lock_guard<std::mutex> _(graphMutex_);
559  base::Goal *g = pdef_->getGoal().get();
560  base::Cost closestVal(opt_->infiniteCost());
561  bool approxPathJustStart = true;
562 
563  foreach (Vertex start, starts)
564  {
565  foreach (Vertex goal, goals)
566  {
567  base::Cost heuristicCost(costHeuristic(start, goal));
568  if (opt_->isCostBetterThan(heuristicCost, closestVal))
569  {
570  closestVal = heuristicCost;
571  approxPathJustStart = true;
572  }
573  if (!g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
574  {
575  continue;
576  }
577  base::PathPtr p;
578  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
579  boost::vector_property_map<base::Cost> dist(boost::num_vertices(g_));
580  boost::vector_property_map<base::Cost> rank(boost::num_vertices(g_));
581 
582  try
583  {
584  // Consider using a persistent distance_map if it's slow
585  boost::astar_search(
586  g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
587  boost::predecessor_map(prev)
588  .distance_map(dist)
589  .rank_map(rank)
590  .distance_compare(
591  [this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
592  .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
593  .distance_inf(opt_->infiniteCost())
594  .distance_zero(opt_->identityCost())
595  .visitor(AStarGoalVisitor<Vertex>(goal)));
596  }
597  catch (AStarFoundGoal &)
598  {
599  }
600 
601  Vertex closeToGoal = start;
602  for (auto vp = vertices(g_); vp.first != vp.second; vp.first++)
603  {
604  // We want to get the distance of each vertex to the goal.
605  // Boost lets us get cost-to-come, cost-to-come+dist-to-goal,
606  // but not just dist-to-goal.
607  ompl::base::Cost dist_to_goal(costHeuristic(*vp.first, goal));
608  if (opt_->isFinite(rank[*vp.first]) && opt_->isCostBetterThan(dist_to_goal, closestVal))
609  {
610  closeToGoal = *vp.first;
611  closestVal = dist_to_goal;
612  approxPathJustStart = false;
613  }
614  }
615  if (closeToGoal != start)
616  {
617  auto p(std::make_shared<PathGeometric>(si_));
618  for (Vertex pos = closeToGoal; prev[pos] != pos; pos = prev[pos])
619  p->append(stateProperty_[pos]);
620  p->append(stateProperty_[start]);
621  p->reverse();
622 
623  solution = p;
624  }
625  }
626  }
627  if (approxPathJustStart)
628  {
629  return opt_->infiniteCost();
630  }
631  return closestVal;
632 }
633 
635 {
636  std::lock_guard<std::mutex> _(graphMutex_);
637  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
638 
639  try
640  {
641  // Consider using a persistent distance_map if it's slow
642  boost::astar_search(
643  g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
644  boost::predecessor_map(prev)
645  .distance_compare([this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
646  .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
647  .distance_inf(opt_->infiniteCost())
648  .distance_zero(opt_->identityCost())
649  .visitor(AStarGoalVisitor<Vertex>(goal)));
650  }
651  catch (AStarFoundGoal &)
652  {
653  }
654 
655  if (prev[goal] == goal)
656  throw Exception(name_, "Could not find solution path");
657 
658  auto p(std::make_shared<PathGeometric>(si_));
659  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
660  p->append(stateProperty_[pos]);
661  p->append(stateProperty_[start]);
662  p->reverse();
663 
664  return p;
665 }
666 
668 {
669  Planner::getPlannerData(data);
670 
671  // Explicitly add start and goal states:
672  for (unsigned long i : startM_)
673  data.addStartVertex(
674  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
675 
676  for (unsigned long i : goalM_)
677  data.addGoalVertex(
678  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
679 
680  // Adding edges and all other vertices simultaneously
681  foreach (const Edge e, boost::edges(g_))
682  {
683  const Vertex v1 = boost::source(e, g_);
684  const Vertex v2 = boost::target(e, g_);
685  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
686 
687  // Add the reverse edge, since we're constructing an undirected roadmap
688  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
689 
690  // Add tags for the newly added vertices
691  data.tagState(stateProperty_[v1], const_cast<PRM *>(this)->disjointSets_.find_set(v1));
692  data.tagState(stateProperty_[v2], const_cast<PRM *>(this)->disjointSets_.find_set(v2));
693  }
694 }
695 
697 {
698  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
699 }
ompl::base::Cost constructApproximateSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
(Assuming that there is always an approximate solution), finds an approximate solution.
Definition: PRM.cpp:553
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:193
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element,...
Definition: PDF.h:158
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
The goal is of a type that a planner does not recognize.
A shared pointer wrapper for ompl::base::Path.
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:261
The planner found an approximate solution.
void growRoadmap(double growTime)
If the user desires, the roadmap can be improved for the given time (seconds). The solve() method wil...
Definition: PRM.cpp:284
void setDefaultConnectionStrategy()
Definition: PRM.cpp:161
Definition of an abstract state.
Definition: State.h:110
base::PathPtr constructSolution(const Vertex &start, const Vertex &goal)
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition: PRM.cpp:633
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:472
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:324
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: PRM.cpp:666
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition: PRM.cpp:138
double value() const
The value of the cost.
Definition: Cost.h:148
Representation of a solution to a planning problem.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:108
bool tagState(const State *st, int tag)
Set the integer tag associated with the given state. If the given state does not exist in a vertex,...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:67
The planner failed to find a solution.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:78
Make the minimal number of connections required to ensure asymptotic optimality.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PRM.cpp:100
bool empty() const
Returns whether the PDF contains no data.
Definition: PDF.h:324
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition: PRM.cpp:203
unsigned int getMaxNearestNeighbors() const
return the maximum number of nearest neighbors to connect a sample to
Definition: PRM.cpp:155
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:235
Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition: PRM.cpp:507
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. Grows a roadmap using constructRoadmap()....
Definition: PRM.cpp:391
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:491
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:268
bool maybeConstructSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition: PRM.cpp:345
A class to store the exit status of Planner::solve()
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition: PRM.h:157
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:386
static const unsigned int DEFAULT_NEAREST_NEIGHBORS
The number of nearest neighbors to consider by default in the construction of the PRM roadmap.
Definition: PRM.cpp:67
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:475
static const unsigned int MAX_RANDOM_BOUNCE_STEPS
The number of steps to take for a random bounce motion generated as part of the expansion step of PRM...
Definition: PRM.cpp:60
void freeMemory()
Free all the memory allocated by the planner.
Definition: PRM.cpp:196
Abstract definition of goals.
Definition: Goal.h:123
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:148
The planner found an exact solution.
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true,...
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: PRM.h:397
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:71
static const double ROADMAP_BUILD_TIME
The time in seconds for a single roadmap building operation (dt)
Definition: PRM.cpp:63
void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
Set the optimization objective used to optimize this solution, the cost of the solution and whether i...
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...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:63
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:264
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
Definition: Goal.h:197
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: PRM.cpp:175
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: PRM.h:155
void uniteComponents(Vertex m1, Vertex m2)
Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...
Definition: PRM.cpp:543
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: PRM.cpp:695
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PRM.cpp:182
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...
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...
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 ...
Probabilistic RoadMap planner.
Definition: PRM.h:110
Abstract definition of a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:76
Invalid start state or no start state specified.
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: PRM.cpp:548
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:119
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:258
Main namespace. Contains everything in this library.
Definition: AppBase.h:20