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  , userSetConnectionStrategy_(false)
80  , addedNewSolution_(false)
81  , iterations_(0)
82  , bestCost_(std::numeric_limits<double>::quiet_NaN())
83 {
86  specs_.optimizingPaths = true;
87  specs_.multithreaded = true;
88 
89  if (!starStrategy_)
90  Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors,
91  std::string("8:1000"));
92 
93  addPlannerProgressProperty("iterations INTEGER", [this]
94  {
95  return getIterationCount();
96  });
97  addPlannerProgressProperty("best cost REAL", [this]
98  {
99  return getBestCost();
100  });
101  addPlannerProgressProperty("milestone count INTEGER", [this]
102  {
103  return getMilestoneCountString();
104  });
105  addPlannerProgressProperty("edge count INTEGER", [this]
106  {
107  return getEdgeCountString();
108  });
109 }
110 
111 ompl::geometric::PRM::~PRM()
112 {
113  freeMemory();
114 }
115 
117 {
118  Planner::setup();
119  if (!nn_)
120  {
121  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
122  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
123  specs_.multithreaded = true;
124  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
125  {
126  return distanceFunction(a, b);
127  });
128  }
129  if (!connectionStrategy_)
130  {
131  if (starStrategy_)
133  [this]
134  {
135  return milestoneCount();
136  },
137  nn_, si_->getStateDimension());
138  else
140  }
141  if (!connectionFilter_)
142  connectionFilter_ = [](const Vertex &, const Vertex &)
143  {
144  return true;
145  };
146 
147  // Setup optimization objective
148  //
149  // If no optimization objective was specified, then default to
150  // optimizing path length as computed by the distance() function
151  // in the state space.
152  if (pdef_)
153  {
154  if (pdef_->hasOptimizationObjective())
155  opt_ = pdef_->getOptimizationObjective();
156  else
157  {
158  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
159  if (!starStrategy_)
160  opt_->setCostThreshold(opt_->infiniteCost());
161  }
162  }
163  else
164  {
165  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
166  setup_ = false;
167  }
168 }
169 
171 {
172  if (starStrategy_)
173  throw Exception("Cannot set the maximum nearest neighbors for " + getName());
174  if (!nn_)
175  {
176  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
177  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
178  specs_.multithreaded = true;
179  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
180  {
181  return distanceFunction(a, b);
182  });
183  }
186  if (isSetup())
187  setup();
188 }
189 
191 {
192  Planner::setProblemDefinition(pdef);
193  clearQuery();
194 }
195 
197 {
198  startM_.clear();
199  goalM_.clear();
200  pis_.restart();
201 }
202 
204 {
205  Planner::clear();
206  sampler_.reset();
207  simpleSampler_.reset();
208  freeMemory();
209  if (nn_)
210  nn_->clear();
211  clearQuery();
212 
213  iterations_ = 0;
214  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
215 }
216 
218 {
219  foreach (Vertex v, boost::vertices(g_))
220  si_->freeState(stateProperty_[v]);
221  g_.clear();
222 }
223 
224 void ompl::geometric::PRM::expandRoadmap(double expandTime)
225 {
227 }
228 
230 {
231  if (!simpleSampler_)
232  simpleSampler_ = si_->allocStateSampler();
233 
234  std::vector<base::State *> states(magic::MAX_RANDOM_BOUNCE_STEPS);
235  si_->allocStates(states);
236  expandRoadmap(ptc, states);
237  si_->freeStates(states);
238 }
239 
241  std::vector<base::State *> &workStates)
242 {
243  // construct a probability distribution over the vertices in the roadmap
244  // as indicated in
245  // "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
246  // Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
247 
248  PDF<Vertex> pdf;
249  foreach (Vertex v, boost::vertices(g_))
250  {
251  const unsigned long int t = totalConnectionAttemptsProperty_[v];
252  pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
253  }
254 
255  if (pdf.empty())
256  return;
257 
258  while (ptc == false)
259  {
260  iterations_++;
261  Vertex v = pdf.sample(rng_.uniform01());
262  unsigned int s =
263  si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
264  if (s > 0)
265  {
266  s--;
267  Vertex last = addMilestone(si_->cloneState(workStates[s]));
268 
269  graphMutex_.lock();
270  for (unsigned int i = 0; i < s; ++i)
271  {
272  // add the vertex along the bouncing motion
273  Vertex m = boost::add_vertex(g_);
274  stateProperty_[m] = si_->cloneState(workStates[i]);
277  disjointSets_.make_set(m);
278 
279  // add the edge to the parent vertex
280  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
281  const Graph::edge_property_type properties(weight);
282  boost::add_edge(v, m, properties, g_);
283  uniteComponents(v, m);
284 
285  // add the vertex to the nearest neighbors data structure
286  nn_->add(m);
287  v = m;
288  }
289 
290  // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
291  // we add an edge
292  if (s > 0 || !sameComponent(v, last))
293  {
294  // add the edge to the parent vertex
295  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
296  const Graph::edge_property_type properties(weight);
297  boost::add_edge(v, last, properties, g_);
298  uniteComponents(v, last);
299  }
300  graphMutex_.unlock();
301  }
302  }
303 }
304 
306 {
308 }
309 
311 {
312  if (!isSetup())
313  setup();
314  if (!sampler_)
315  sampler_ = si_->allocValidStateSampler();
316 
317  base::State *workState = si_->allocState();
318  growRoadmap(ptc, workState);
319  si_->freeState(workState);
320 }
321 
323 {
324  /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
325  while (ptc == false)
326  {
327  iterations_++;
328  // search for a valid state
329  bool found = false;
330  while (!found && ptc == false)
331  {
332  unsigned int attempts = 0;
333  do
334  {
335  found = sampler_->sample(workState);
336  attempts++;
337  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
338  }
339  // add it as a milestone
340  if (found)
341  addMilestone(si_->cloneState(workState));
342  }
343 }
344 
346 {
347  base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
348  while (!ptc && !addedNewSolution_)
349  {
350  // Check for any new goal states
351  if (goal->maxSampleCount() > goalM_.size())
352  {
353  const base::State *st = pis_.nextGoal();
354  if (st)
355  goalM_.push_back(addMilestone(si_->cloneState(st)));
356  }
357 
358  // Check for a solution
360  // Sleep for 1ms
361  if (!addedNewSolution_)
362  std::this_thread::sleep_for(std::chrono::milliseconds(1));
363  }
364 }
365 
366 bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
367  base::PathPtr &solution)
368 {
369  base::Goal *g = pdef_->getGoal().get();
370  base::Cost sol_cost(opt_->infiniteCost());
371  foreach (Vertex start, starts)
372  {
373  foreach (Vertex goal, goals)
374  {
375  // we lock because the connected components algorithm is incremental and may change disjointSets_
376  graphMutex_.lock();
377  bool same_component = sameComponent(start, goal);
378  graphMutex_.unlock();
379 
380  if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
381  {
382  base::PathPtr p = constructSolution(start, goal);
383  if (p)
384  {
385  base::Cost pathCost = p->cost(opt_);
386  if (opt_->isCostBetterThan(pathCost, bestCost_))
387  bestCost_ = pathCost;
388  // Check if optimization objective is satisfied
389  if (opt_->isSatisfied(pathCost))
390  {
391  solution = p;
392  return true;
393  }
394  else if (opt_->isCostBetterThan(pathCost, sol_cost))
395  {
396  solution = p;
397  sol_cost = pathCost;
398  }
399  }
400  }
401  }
402  }
403 
404  return false;
405 }
406 
408 {
409  return addedNewSolution_;
410 }
411 
413 {
414  checkValidity();
415  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
416 
417  if (!goal)
418  {
419  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
421  }
422 
423  // Add the valid start states as milestones
424  while (const base::State *st = pis_.nextStart())
425  startM_.push_back(addMilestone(si_->cloneState(st)));
426 
427  if (startM_.size() == 0)
428  {
429  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
431  }
432 
433  if (!goal->couldSample())
434  {
435  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
437  }
438 
439  // Ensure there is at least one valid goal state
440  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
441  {
442  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
443  if (st)
444  goalM_.push_back(addMilestone(si_->cloneState(st)));
445 
446  if (goalM_.empty())
447  {
448  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
450  }
451  }
452 
453  unsigned long int nrStartStates = boost::num_vertices(g_);
454  OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
455 
456  // Reset addedNewSolution_ member and create solution checking thread
457  addedNewSolution_ = false;
458  base::PathPtr sol;
459  std::thread slnThread([this, &ptc, &sol]
460  {
461  checkForSolution(ptc, sol);
462  });
463 
464  // construct new planner termination condition that fires when the given ptc is true, or a solution is found
465  base::PlannerTerminationCondition ptcOrSolutionFound([this, &ptc]
466  {
467  return ptc || addedNewSolution();
468  });
469 
470  constructRoadmap(ptcOrSolutionFound);
471 
472  // Ensure slnThread is ceased before exiting solve
473  slnThread.join();
474 
475  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
476 
477  if (sol)
478  {
479  base::PlannerSolution psol(sol);
480  psol.setPlannerName(getName());
481  // if the solution was optimized, we mark it as such
483  pdef_->addSolutionPath(psol);
484  }
485 
487 }
488 
490 {
491  if (!isSetup())
492  setup();
493  if (!sampler_)
494  sampler_ = si_->allocValidStateSampler();
495  if (!simpleSampler_)
496  simpleSampler_ = si_->allocStateSampler();
497 
498  std::vector<base::State *> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
499  si_->allocStates(xstates);
500  bool grow = true;
501 
502  bestCost_ = opt_->infiniteCost();
503  while (ptc() == false)
504  {
505  // maintain a 2:1 ratio for growing/expansion of roadmap
506  // call growRoadmap() twice as long for every call of expandRoadmap()
507  if (grow)
510  xstates[0]);
511  else
514  xstates);
515  grow = !grow;
516  }
517 
518  si_->freeStates(xstates);
519 }
520 
522 {
523  std::lock_guard<std::mutex> _(graphMutex_);
524 
525  Vertex m = boost::add_vertex(g_);
526  stateProperty_[m] = state;
529 
530  // Initialize to its own (dis)connected component.
531  disjointSets_.make_set(m);
532 
533  // Which milestones will we attempt to connect to?
534  const std::vector<Vertex> &neighbors = connectionStrategy_(m);
535 
536  foreach (Vertex n, neighbors)
537  if (connectionFilter_(n, m))
538  {
541  if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
542  {
545  const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
546  const Graph::edge_property_type properties(weight);
547  boost::add_edge(n, m, properties, g_);
548  uniteComponents(n, m);
549  }
550  }
551 
552  nn_->add(m);
553 
554  return m;
555 }
556 
558 {
559  disjointSets_.union_set(m1, m2);
560 }
561 
563 {
564  return boost::same_component(m1, m2, disjointSets_);
565 }
566 
568 {
569  std::lock_guard<std::mutex> _(graphMutex_);
570  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
571 
572  try
573  {
574  // Consider using a persistent distance_map if it's slow
575  boost::astar_search(g_, start,
576  [this, goal](Vertex v)
577  {
578  return costHeuristic(v, goal);
579  },
580  boost::predecessor_map(prev)
581  .distance_compare([this](base::Cost c1, base::Cost c2)
582  {
583  return opt_->isCostBetterThan(c1, c2);
584  })
585  .distance_combine([this](base::Cost c1, base::Cost c2)
586  {
587  return opt_->combineCosts(c1, c2);
588  })
589  .distance_inf(opt_->infiniteCost())
590  .distance_zero(opt_->identityCost())
591  .visitor(AStarGoalVisitor<Vertex>(goal)));
592  }
593  catch (AStarFoundGoal &)
594  {
595  }
596 
597  if (prev[goal] == goal)
598  throw Exception(name_, "Could not find solution path");
599 
600  auto p(std::make_shared<PathGeometric>(si_));
601  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
602  p->append(stateProperty_[pos]);
603  p->append(stateProperty_[start]);
604  p->reverse();
605 
606  return p;
607 }
608 
610 {
611  Planner::getPlannerData(data);
612 
613  // Explicitly add start and goal states:
614  for (unsigned long i : startM_)
615  data.addStartVertex(
616  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
617 
618  for (unsigned long i : goalM_)
619  data.addGoalVertex(
620  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
621 
622  // Adding edges and all other vertices simultaneously
623  foreach (const Edge e, boost::edges(g_))
624  {
625  const Vertex v1 = boost::source(e, g_);
626  const Vertex v2 = boost::target(e, g_);
628 
629  // Add the reverse edge, since we're constructing an undirected roadmap
631 
632  // Add tags for the newly added vertices
633  data.tagState(stateProperty_[v1], const_cast<PRM *>(this)->disjointSets_.find_set(v1));
634  data.tagState(stateProperty_[v2], const_cast<PRM *>(this)->disjointSets_.find_set(v2));
635  }
636 }
637 
639 {
640  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
641 }
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:212
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:408
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:196
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:345
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:206
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:638
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
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:412
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:224
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:269
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:562
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:116
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
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:305
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:418
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:209
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:407
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:609
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:438
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
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
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:217
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:521
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:567
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:421
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:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
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:190
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:203
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:557
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:424
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:170
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:216
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:366
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:171
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:415
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:489
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