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, Henning Kayser */
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(const base::PlannerData &data, bool starStrategy)
97  : PRM(data.getSpaceInformation(), starStrategy)
98 {
99  if (data.numVertices() > 0)
100  {
101  // mapping between vertex id from PlannerData and Vertex in Boost.Graph
102  std::map<unsigned int, Vertex> vertices;
103  // helper function to create vertices as needed and update the vertices mapping
104  const auto &getOrCreateVertex = [&](unsigned int vertex_index)
105  {
106  if (!vertices.count(vertex_index))
107  {
108  const auto &data_vertex = data.getVertex(vertex_index);
109  Vertex graph_vertex = boost::add_vertex(g_);
110  stateProperty_[graph_vertex] = si_->cloneState(data_vertex.getState());
111  totalConnectionAttemptsProperty_[graph_vertex] = 1;
112  successfulConnectionAttemptsProperty_[graph_vertex] = 0;
113  vertices[vertex_index] = graph_vertex;
114  }
115  return vertices.at(vertex_index);
116  };
117 
118  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
119  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
120  specs_.multithreaded = true;
121  nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
122 
123  // Initialize Disjoint Sets
124  for (size_t vertex_index = 0; vertex_index < data.numVertices(); ++vertex_index)
125  {
126  Vertex m = getOrCreateVertex(vertex_index);
127  disjointSets_.make_set(m); // Initialize each vertex as a separate set
128  }
129 
130  for (size_t vertex_index = 0; vertex_index < data.numVertices(); ++vertex_index)
131  {
132  Vertex m = getOrCreateVertex(vertex_index);
133  std::vector<unsigned int> neighbor_indices;
134  data.getEdges(vertex_index, neighbor_indices);
135 
136  for (const unsigned int neighbor_index : neighbor_indices)
137  {
138  Vertex n = getOrCreateVertex(neighbor_index);
139 
142 
143  base::Cost weight;
144  data.getEdgeWeight(vertex_index, neighbor_index,
145  &weight); // Get the weight of the edge
146  const Graph::edge_property_type properties(weight);
147 
148  boost::add_edge(m, n, properties, g_); // Add the edge to the graph
149 
150  uniteComponents(m, n);
151  }
152 
153  nn_->add(m); // Add the vertex to the nearest neighbor data structure
154  }
155  }
156 }
157 
158 ompl::geometric::PRM::~PRM()
159 {
160  freeMemory();
161 }
162 
164 {
165  Planner::setup();
166  if (!nn_)
167  {
168  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
169  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
170  specs_.multithreaded = true;
171  nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
172  }
173  if (!connectionStrategy_)
174  setDefaultConnectionStrategy();
175  if (!connectionFilter_)
176  connectionFilter_ = [](const Vertex &, const Vertex &) { return true; };
177 
178  // Setup optimization objective
179  //
180  // If no optimization objective was specified, then default to
181  // optimizing path length as computed by the distance() function
182  // in the state space.
183  if (pdef_)
184  {
185  if (pdef_->hasOptimizationObjective())
186  opt_ = pdef_->getOptimizationObjective();
187  else
188  {
189  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
190  if (!starStrategy_)
191  opt_->setCostThreshold(opt_->infiniteCost());
192  }
193  }
194  else
195  {
196  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
197  setup_ = false;
198  }
199 }
200 
202 {
203  if (starStrategy_)
204  throw Exception("Cannot set the maximum nearest neighbors for " + getName());
205  if (!nn_)
206  {
207  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
208  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
209  specs_.multithreaded = true;
210  nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
211  }
212  if (!userSetConnectionStrategy_)
213  connectionStrategy_ = KStrategy<Vertex>(k, nn_);
214  if (isSetup())
215  setup();
216 }
217 
219 {
220  const auto strategy = connectionStrategy_.target<KStrategy<Vertex>>();
221  return strategy ? strategy->getNumNeighbors() : 0u;
222 }
223 
225 {
226  if (starStrategy_)
227  connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
228  else
229  connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
230 }
231 
232 void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
233 {
234  Planner::setProblemDefinition(pdef);
235  clearQuery();
236 }
237 
239 {
240  startM_.clear();
241  goalM_.clear();
242  pis_.restart();
243 }
244 
246 {
247  Planner::clear();
248  sampler_.reset();
249  simpleSampler_.reset();
250  freeMemory();
251  if (nn_)
252  nn_->clear();
253  clearQuery();
254 
255  iterations_ = 0;
256  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
257 }
258 
260 {
261  foreach (Vertex v, boost::vertices(g_))
262  si_->freeState(stateProperty_[v]);
263  g_.clear();
264 }
265 
266 void ompl::geometric::PRM::expandRoadmap(double expandTime)
267 {
268  expandRoadmap(base::timedPlannerTerminationCondition(expandTime));
269 }
270 
272 {
273  if (!simpleSampler_)
274  simpleSampler_ = si_->allocStateSampler();
275 
276  std::vector<base::State *> states(magic::MAX_RANDOM_BOUNCE_STEPS);
277  si_->allocStates(states);
278  expandRoadmap(ptc, states);
279  si_->freeStates(states);
280 }
281 
283  std::vector<base::State *> &workStates)
284 {
285  // construct a probability distribution over the vertices in the roadmap
286  // as indicated in
287  // "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
288  // Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
289 
290  PDF<Vertex> pdf;
291  foreach (Vertex v, boost::vertices(g_))
292  {
293  const unsigned long int t = totalConnectionAttemptsProperty_[v];
294  pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
295  }
296 
297  if (pdf.empty())
298  return;
299 
300  while (!ptc)
301  {
302  iterations_++;
303  Vertex v = pdf.sample(rng_.uniform01());
304  unsigned int s =
305  si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
306  if (s > 0)
307  {
308  s--;
309  Vertex last = addMilestone(si_->cloneState(workStates[s]));
310 
311  graphMutex_.lock();
312  for (unsigned int i = 0; i < s; ++i)
313  {
314  // add the vertex along the bouncing motion
315  Vertex m = boost::add_vertex(g_);
316  stateProperty_[m] = si_->cloneState(workStates[i]);
317  totalConnectionAttemptsProperty_[m] = 1;
318  successfulConnectionAttemptsProperty_[m] = 0;
319  disjointSets_.make_set(m);
320 
321  // add the edge to the parent vertex
322  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
323  const Graph::edge_property_type properties(weight);
324  boost::add_edge(v, m, properties, g_);
325  uniteComponents(v, m);
326 
327  // add the vertex to the nearest neighbors data structure
328  nn_->add(m);
329  v = m;
330  }
331 
332  // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
333  // we add an edge
334  if (s > 0 || !sameComponent(v, last))
335  {
336  // add the edge to the parent vertex
337  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
338  const Graph::edge_property_type properties(weight);
339  boost::add_edge(v, last, properties, g_);
340  uniteComponents(v, last);
341  }
342  graphMutex_.unlock();
343  }
344  }
345 }
346 
348 {
349  growRoadmap(base::timedPlannerTerminationCondition(growTime));
350 }
351 
353 {
354  if (!isSetup())
355  setup();
356  if (!sampler_)
357  sampler_ = si_->allocValidStateSampler();
358 
359  base::State *workState = si_->allocState();
360  growRoadmap(ptc, workState);
361  si_->freeState(workState);
362 }
363 
365 {
366  /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
367  while (!ptc)
368  {
369  iterations_++;
370  // search for a valid state
371  bool found = false;
372  while (!found && !ptc)
373  {
374  unsigned int attempts = 0;
375  do
376  {
377  found = sampler_->sample(workState);
378  attempts++;
379  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
380  }
381  // add it as a milestone
382  if (found)
383  addMilestone(si_->cloneState(workState));
384  }
385 }
386 
388 {
389  auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
390  while (!ptc && !addedNewSolution_)
391  {
392  // Check for any new goal states
393  if (goal->maxSampleCount() > goalM_.size())
394  {
395  const base::State *st = pis_.nextGoal();
396  if (st != nullptr)
397  goalM_.push_back(addMilestone(si_->cloneState(st)));
398  }
399 
400  // Check for a solution
401  addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution);
402  // Sleep for 1ms
403  if (!addedNewSolution_)
404  std::this_thread::sleep_for(std::chrono::milliseconds(1));
405  }
406 }
407 
408 bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
409  base::PathPtr &solution)
410 {
411  base::Goal *g = pdef_->getGoal().get();
412  base::Cost sol_cost(opt_->infiniteCost());
413  foreach (Vertex start, starts)
414  {
415  foreach (Vertex goal, goals)
416  {
417  // we lock because the connected components algorithm is incremental and may change disjointSets_
418  graphMutex_.lock();
419  bool same_component = sameComponent(start, goal);
420  graphMutex_.unlock();
421 
422  if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
423  {
424  base::PathPtr p = constructSolution(start, goal);
425  if (p)
426  {
427  base::Cost pathCost = p->cost(opt_);
428  if (opt_->isCostBetterThan(pathCost, bestCost_))
429  bestCost_ = pathCost;
430  // Check if optimization objective is satisfied
431  if (opt_->isSatisfied(pathCost))
432  {
433  solution = p;
434  return true;
435  }
436  if (opt_->isCostBetterThan(pathCost, sol_cost))
437  {
438  solution = p;
439  sol_cost = pathCost;
440  }
441  }
442  }
443  }
444  }
445 
446  return false;
447 }
448 
450 {
451  return addedNewSolution_;
452 }
453 
455 {
456  checkValidity();
457  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
458 
459  if (goal == nullptr)
460  {
461  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
463  }
464 
465  // Add the valid start states as milestones
466  while (const base::State *st = pis_.nextStart())
467  startM_.push_back(addMilestone(si_->cloneState(st)));
468 
469  if (startM_.empty())
470  {
471  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
473  }
474 
475  if (!goal->couldSample())
476  {
477  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
479  }
480 
481  // Ensure there is at least one valid goal state
482  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
483  {
484  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
485  if (st != nullptr)
486  goalM_.push_back(addMilestone(si_->cloneState(st)));
487 
488  if (goalM_.empty())
489  {
490  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
492  }
493  }
494 
495  unsigned long int nrStartStates = boost::num_vertices(g_);
496  OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
497 
498  // Reset addedNewSolution_ member and create solution checking thread
499  addedNewSolution_ = false;
500  base::PathPtr sol;
501  std::thread slnThread([this, &ptc, &sol] { checkForSolution(ptc, sol); });
502 
503  // construct new planner termination condition that fires when the given ptc is true, or a solution is found
504  base::PlannerTerminationCondition ptcOrSolutionFound([this, &ptc] { return ptc || addedNewSolution(); });
505 
506  constructRoadmap(ptcOrSolutionFound);
507 
508  // Ensure slnThread is ceased before exiting solve
509  slnThread.join();
510 
511  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
512 
513  if (sol)
514  {
515  base::PlannerSolution psol(sol);
516  psol.setPlannerName(getName());
517  // if the solution was optimized, we mark it as such
518  psol.setOptimized(opt_, bestCost_, addedNewSolution());
519  pdef_->addSolutionPath(psol);
520  }
521  else
522  {
523  // Return an approximate solution.
524  ompl::base::Cost diff = constructApproximateSolution(startM_, goalM_, sol);
525  if (!opt_->isFinite(diff))
526  {
527  OMPL_INFORM("Closest path is still start and goal");
529  }
530  OMPL_INFORM("Using approximate solution, heuristic cost-to-go is %f", diff.value());
531  pdef_->addSolutionPath(sol, true, diff.value(), getName());
533  }
534 
536 }
537 
539 {
540  if (!isSetup())
541  setup();
542  if (!sampler_)
543  sampler_ = si_->allocValidStateSampler();
544  if (!simpleSampler_)
545  simpleSampler_ = si_->allocStateSampler();
546 
547  std::vector<base::State *> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
548  si_->allocStates(xstates);
549  bool grow = true;
550 
551  bestCost_ = opt_->infiniteCost();
552  while (!ptc())
553  {
554  // maintain a 2:1 ratio for growing/expansion of roadmap
555  // call growRoadmap() twice as long for every call of expandRoadmap()
556  if (grow)
559  xstates[0]);
560  else
563  xstates);
564  grow = !grow;
565  }
566 
567  si_->freeStates(xstates);
568 }
569 
571 {
572  std::lock_guard<std::mutex> _(graphMutex_);
573 
574  Vertex m = boost::add_vertex(g_);
575  stateProperty_[m] = state;
576  totalConnectionAttemptsProperty_[m] = 1;
577  successfulConnectionAttemptsProperty_[m] = 0;
578 
579  // Initialize to its own (dis)connected component.
580  disjointSets_.make_set(m);
581 
582  // Which milestones will we attempt to connect to?
583  const std::vector<Vertex> &neighbors = connectionStrategy_(m);
584 
585  foreach (Vertex n, neighbors)
586  if (connectionFilter_(n, m))
587  {
588  totalConnectionAttemptsProperty_[m]++;
589  totalConnectionAttemptsProperty_[n]++;
590  if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
591  {
592  successfulConnectionAttemptsProperty_[m]++;
593  successfulConnectionAttemptsProperty_[n]++;
594  const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
595  const Graph::edge_property_type properties(weight);
596  boost::add_edge(n, m, properties, g_);
597  uniteComponents(n, m);
598  }
599  }
600 
601  nn_->add(m);
602 
603  return m;
604 }
605 
607 {
608  disjointSets_.union_set(m1, m2);
609 }
610 
612 {
613  return boost::same_component(m1, m2, disjointSets_);
614 }
615 
617  const std::vector<Vertex> &goals,
618  base::PathPtr &solution)
619 {
620  std::lock_guard<std::mutex> _(graphMutex_);
621  base::Goal *g = pdef_->getGoal().get();
622  base::Cost closestVal(opt_->infiniteCost());
623  bool approxPathJustStart = true;
624 
625  foreach (Vertex start, starts)
626  {
627  foreach (Vertex goal, goals)
628  {
629  base::Cost heuristicCost(costHeuristic(start, goal));
630  if (opt_->isCostBetterThan(heuristicCost, closestVal))
631  {
632  closestVal = heuristicCost;
633  approxPathJustStart = true;
634  }
635  if (!g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
636  {
637  continue;
638  }
639  base::PathPtr p;
640  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
641  boost::vector_property_map<base::Cost> dist(boost::num_vertices(g_));
642  boost::vector_property_map<base::Cost> rank(boost::num_vertices(g_));
643 
644  try
645  {
646  // Consider using a persistent distance_map if it's slow
647  boost::astar_search(
648  g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
649  boost::predecessor_map(prev)
650  .distance_map(dist)
651  .rank_map(rank)
652  .distance_compare(
653  [this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
654  .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
655  .distance_inf(opt_->infiniteCost())
656  .distance_zero(opt_->identityCost())
657  .visitor(AStarGoalVisitor<Vertex>(goal)));
658  }
659  catch (AStarFoundGoal &)
660  {
661  }
662 
663  Vertex closeToGoal = start;
664  for (auto vp = vertices(g_); vp.first != vp.second; vp.first++)
665  {
666  // We want to get the distance of each vertex to the goal.
667  // Boost lets us get cost-to-come, cost-to-come+dist-to-goal,
668  // but not just dist-to-goal.
669  ompl::base::Cost dist_to_goal(costHeuristic(*vp.first, goal));
670  if (opt_->isFinite(rank[*vp.first]) && opt_->isCostBetterThan(dist_to_goal, closestVal))
671  {
672  closeToGoal = *vp.first;
673  closestVal = dist_to_goal;
674  approxPathJustStart = false;
675  }
676  }
677  if (closeToGoal != start)
678  {
679  auto p(std::make_shared<PathGeometric>(si_));
680  for (Vertex pos = closeToGoal; prev[pos] != pos; pos = prev[pos])
681  p->append(stateProperty_[pos]);
682  p->append(stateProperty_[start]);
683  p->reverse();
684 
685  solution = p;
686  }
687  }
688  }
689  if (approxPathJustStart)
690  {
691  return opt_->infiniteCost();
692  }
693  return closestVal;
694 }
695 
697 {
698  std::lock_guard<std::mutex> _(graphMutex_);
699  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
700 
701  try
702  {
703  // Consider using a persistent distance_map if it's slow
704  boost::astar_search(
705  g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
706  boost::predecessor_map(prev)
707  .distance_compare([this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
708  .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
709  .distance_inf(opt_->infiniteCost())
710  .distance_zero(opt_->identityCost())
711  .visitor(AStarGoalVisitor<Vertex>(goal)));
712  }
713  catch (AStarFoundGoal &)
714  {
715  }
716 
717  if (prev[goal] == goal)
718  throw Exception(name_, "Could not find solution path");
719 
720  auto p(std::make_shared<PathGeometric>(si_));
721  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
722  p->append(stateProperty_[pos]);
723  p->append(stateProperty_[start]);
724  p->reverse();
725 
726  return p;
727 }
728 
730 {
731  Planner::getPlannerData(data);
732 
733  // Explicitly add start and goal states:
734  for (unsigned long i : startM_)
735  data.addStartVertex(
736  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
737 
738  for (unsigned long i : goalM_)
739  data.addGoalVertex(
740  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
741 
742  // Adding edges and all other vertices simultaneously
743  foreach (const Edge e, boost::edges(g_))
744  {
745  const Vertex v1 = boost::source(e, g_);
746  const Vertex v2 = boost::target(e, g_);
747  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
748 
749  // Add the reverse edge, since we're constructing an undirected roadmap
750  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
751 
752  // Add tags for the newly added vertices
753  data.tagState(stateProperty_[v1], const_cast<PRM *>(this)->disjointSets_.find_set(v1));
754  data.tagState(stateProperty_[v2], const_cast<PRM *>(this)->disjointSets_.find_set(v2));
755  }
756 }
757 
759 {
760  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
761 }
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:616
_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:196
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:161
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
A shared pointer wrapper for ompl::base::Path.
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: PRM.h:423
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:256
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)....
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:347
void setDefaultConnectionStrategy()
Definition: PRM.cpp:224
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: PRM.h:377
Definition of an abstract state.
Definition: State.h:113
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:696
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:467
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:387
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:729
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: PRM.h:437
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition: PRM.cpp:201
double value() const
The value of the cost.
Definition: Cost.h:152
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:111
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:68
@ TIMEOUT
The planner failed to find a solution.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:80
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:163
bool empty() const
Returns whether the PDF contains no data.
Definition: PDF.h:327
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition: PRM.cpp:266
unsigned int getMaxNearestNeighbors() const
return the maximum number of nearest neighbors to connect a sample to
Definition: PRM.cpp:218
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
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:570
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. Grows a roadmap using constructRoadmap()....
Definition: PRM.cpp:454
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
@ INVALID_GOAL
Invalid goal state.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:263
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:408
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:159
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:449
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:68
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:538
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:61
void freeMemory()
Free all the memory allocated by the planner.
Definition: PRM.cpp:259
Abstract definition of goals.
Definition: Goal.h:126
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
boost::property_map< Graph, vertex_successful_connection_attempts_t >::type successfulConnectionAttemptsProperty_
Access to the number of successful connection attempts for a vertex.
Definition: PRM.h:430
@ EXACT_SOLUTION
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:402
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:72
static const double ROADMAP_BUILD_TIME
The time in seconds for a single roadmap building operation (dt)
Definition: PRM.cpp:64
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...
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: PRM.h:411
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
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:200
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: PRM.cpp:238
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: PRM.h:157
boost::property_map< Graph, vertex_total_connection_attempts_t >::type totalConnectionAttemptsProperty_
Access to the number of total connection attempts for a vertex.
Definition: PRM.h:426
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:606
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:758
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:245
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 ...
Graph g_
Connectivity graph.
Definition: PRM.h:414
Probabilistic RoadMap planner.
Definition: PRM.h:112
Abstract definition of a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:78
@ INVALID_START
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:611
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:253
Main namespace. Contains everything in this library.
Definition: AppBase.h:21