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_)
129  [this]
130  {
131  return milestoneCount();
132  },
133  nn_, si_->getStateDimension());
134  else
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  }
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 {
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]);
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 {
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
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
479  pdef_->addSolutionPath(psol);
480  }
481 
483 }
484 
486 {
487  if (!isSetup())
488  setup();
489  if (!sampler_)
490  sampler_ = si_->allocValidStateSampler();
491  if (!simpleSampler_)
492  simpleSampler_ = si_->allocStateSampler();
493 
494  std::vector<base::State *> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
495  si_->allocStates(xstates);
496  bool grow = true;
497 
498  bestCost_ = opt_->infiniteCost();
499  while (!ptc())
500  {
501  // maintain a 2:1 ratio for growing/expansion of roadmap
502  // call growRoadmap() twice as long for every call of expandRoadmap()
503  if (grow)
506  xstates[0]);
507  else
510  xstates);
511  grow = !grow;
512  }
513 
514  si_->freeStates(xstates);
515 }
516 
518 {
519  std::lock_guard<std::mutex> _(graphMutex_);
520 
521  Vertex m = boost::add_vertex(g_);
522  stateProperty_[m] = state;
525 
526  // Initialize to its own (dis)connected component.
527  disjointSets_.make_set(m);
528 
529  // Which milestones will we attempt to connect to?
530  const std::vector<Vertex> &neighbors = connectionStrategy_(m);
531 
532  foreach (Vertex n, neighbors)
533  if (connectionFilter_(n, m))
534  {
537  if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
538  {
541  const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
542  const Graph::edge_property_type properties(weight);
543  boost::add_edge(n, m, properties, g_);
544  uniteComponents(n, m);
545  }
546  }
547 
548  nn_->add(m);
549 
550  return m;
551 }
552 
554 {
555  disjointSets_.union_set(m1, m2);
556 }
557 
559 {
560  return boost::same_component(m1, m2, disjointSets_);
561 }
562 
564 {
565  std::lock_guard<std::mutex> _(graphMutex_);
566  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
567 
568  try
569  {
570  // Consider using a persistent distance_map if it's slow
571  boost::astar_search(g_, start,
572  [this, goal](Vertex v)
573  {
574  return costHeuristic(v, goal);
575  },
576  boost::predecessor_map(prev)
577  .distance_compare([this](base::Cost c1, base::Cost c2)
578  {
579  return opt_->isCostBetterThan(c1, c2);
580  })
581  .distance_combine([this](base::Cost c1, base::Cost c2)
582  {
583  return opt_->combineCosts(c1, c2);
584  })
585  .distance_inf(opt_->infiniteCost())
586  .distance_zero(opt_->identityCost())
587  .visitor(AStarGoalVisitor<Vertex>(goal)));
588  }
589  catch (AStarFoundGoal &)
590  {
591  }
592 
593  if (prev[goal] == goal)
594  throw Exception(name_, "Could not find solution path");
595 
596  auto p(std::make_shared<PathGeometric>(si_));
597  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
598  p->append(stateProperty_[pos]);
599  p->append(stateProperty_[start]);
600  p->reverse();
601 
602  return p;
603 }
604 
606 {
607  Planner::getPlannerData(data);
608 
609  // Explicitly add start and goal states:
610  for (unsigned long i : startM_)
611  data.addStartVertex(
612  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
613 
614  for (unsigned long i : goalM_)
615  data.addGoalVertex(
616  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
617 
618  // Adding edges and all other vertices simultaneously
619  foreach (const Edge e, boost::edges(g_))
620  {
621  const Vertex v1 = boost::source(e, g_);
622  const Vertex v2 = boost::target(e, g_);
624 
625  // Add the reverse edge, since we're constructing an undirected roadmap
627 
628  // Add tags for the newly added vertices
629  data.tagState(stateProperty_[v1], const_cast<PRM *>(this)->disjointSets_.find_set(v1));
630  data.tagState(stateProperty_[v2], const_cast<PRM *>(this)->disjointSets_.find_set(v2));
631  }
632 }
633 
635 {
636  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
637 }
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:71
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
Definition: PRM.h:264
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
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:329
unsigned long int iterations_
Number of iterations the algorithm performed.
Definition: PRM.h:416
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: PRM.h:372
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
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: PRM.h:357
A shared pointer wrapper for ompl::base::ProblemDefinition.
The planner failed to find a solution.
Definition: PlannerStatus.h:62
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:389
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:634
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
std::mutex graphMutex_
Mutex to guard access to the Graph member (g_)
Definition: PRM.h:408
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition: PRM.cpp:220
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp: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:136
ConnectionStrategy connectionStrategy_
Function that returns the milestones to attempt connections with.
Definition: PRM.h:392
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:558
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 addedNewSolution_
A flag indicating that a solution has been added during solve()
Definition: PRM.h:405
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
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:605
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
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:382
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
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:429
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
std::vector< Vertex > startM_
Array of start milestones.
Definition: PRM.h:369
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:354
#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:517
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:563
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
base::StateSamplerPtr simpleSampler_
Sampler user for generating random in the state space.
Definition: PRM.h:360
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
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
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 ...
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: PRM.h:375
bool userSetConnectionStrategy_
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
Definition: PRM.h:399
Graph g_
Connectivity graph.
Definition: PRM.h:366
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
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
_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:553
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:108
std::string name_
The name of this planner.
Definition: Planner.h:415
ConnectionFilter connectionFilter_
Function that can reject a milestone connection.
Definition: PRM.h:395
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
Definition: PRM.cpp:166
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition: PRM.h:411
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:207
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
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:170
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: PRM.h:418
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
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
RNG rng_
Random number generator.
Definition: PRM.h:402
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:485
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
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: PRM.h:363
A shared pointer wrapper for ompl::base::Path.
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:378
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