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