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/lambda/bind.hpp>
45 #include <boost/graph/astar_search.hpp>
46 #include <boost/graph/incremental_components.hpp>
47 #include <boost/property_map/vector_property_map.hpp>
48 #include <boost/foreach.hpp>
49 #include <boost/thread.hpp>
50 
51 #include "GoalVisitor.hpp"
52 
53 #define foreach BOOST_FOREACH
54 
55 namespace ompl
56 {
57  namespace magic
58  {
59 
62  static const unsigned int MAX_RANDOM_BOUNCE_STEPS = 5;
63 
65  static const double ROADMAP_BUILD_TIME = 0.2;
66 
69  static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
70  }
71 }
72 
74  base::Planner(si, "PRM"),
75  starStrategy_(starStrategy),
76  stateProperty_(boost::get(vertex_state_t(), g_)),
77  totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_)),
78  successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_)),
79  weightProperty_(boost::get(boost::edge_weight, g_)),
80  disjointSets_(boost::get(boost::vertex_rank, g_),
81  boost::get(boost::vertex_predecessor, g_)),
82  userSetConnectionStrategy_(false),
83  addedNewSolution_(false),
84  iterations_(0),
85  bestCost_(std::numeric_limits<double>::quiet_NaN())
86 {
89  specs_.optimizingPaths = true;
90 
91  Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors, std::string("8:1000"));
92 
93  addPlannerProgressProperty("iterations INTEGER",
94  boost::bind(&PRM::getIterationCount, this));
95  addPlannerProgressProperty("best cost REAL",
96  boost::bind(&PRM::getBestCost, this));
97  addPlannerProgressProperty("milestone count INTEGER",
98  boost::bind(&PRM::getMilestoneCountString, this));
99  addPlannerProgressProperty("edge count INTEGER",
100  boost::bind(&PRM::getEdgeCountString, this));
101 }
102 
103 ompl::geometric::PRM::~PRM()
104 {
105  freeMemory();
106 }
107 
109 {
110  Planner::setup();
111  if (!nn_)
112  {
113  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
114  nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
115  }
116  if (!connectionStrategy_)
117  {
118  if (starStrategy_)
119  connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension());
120  else
121  connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
122  }
123  if (!connectionFilter_)
124  connectionFilter_ = boost::lambda::constant(true);
125 
126  // Setup optimization objective
127  //
128  // If no optimization objective was specified, then default to
129  // optimizing path length as computed by the distance() function
130  // in the state space.
131  if (pdef_)
132  {
133  if (pdef_->hasOptimizationObjective())
134  opt_ = pdef_->getOptimizationObjective();
135  else
136  {
137  opt_.reset(new base::PathLengthOptimizationObjective(si_));
138  if (!starStrategy_)
139  opt_->setCostThreshold(opt_->infiniteCost());
140  }
141  }
142  else
143  {
144  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
145  setup_ = false;
146  }
147 }
148 
150 {
151  if (starStrategy_)
152  throw Exception("Cannot set the maximum nearest neighbors for " + getName());
153  if (!nn_)
154  {
155  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
156  nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
157  }
158  if (!userSetConnectionStrategy_)
159  connectionStrategy_.clear();
160  if (isSetup())
161  setup();
162 }
163 
165 {
166  Planner::setProblemDefinition(pdef);
167  clearQuery();
168 }
169 
171 {
172  startM_.clear();
173  goalM_.clear();
174  pis_.restart();
175 }
176 
178 {
179  Planner::clear();
180  sampler_.reset();
181  simpleSampler_.reset();
182  freeMemory();
183  if (nn_)
184  nn_->clear();
185  clearQuery();
186 
187  iterations_ = 0;
188  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
189 }
190 
192 {
193  foreach (Vertex v, boost::vertices(g_))
194  si_->freeState(stateProperty_[v]);
195  g_.clear();
196 }
197 
198 void ompl::geometric::PRM::expandRoadmap(double expandTime)
199 {
200  expandRoadmap(base::timedPlannerTerminationCondition(expandTime));
201 }
202 
204 {
205  if (!simpleSampler_)
206  simpleSampler_ = si_->allocStateSampler();
207 
208  std::vector<base::State*> states(magic::MAX_RANDOM_BOUNCE_STEPS);
209  si_->allocStates(states);
210  expandRoadmap(ptc, states);
211  si_->freeStates(states);
212 }
213 
215  std::vector<base::State*> &workStates)
216 {
217  // construct a probability distribution over the vertices in the roadmap
218  // as indicated in
219  // "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
220  // Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
221 
222  PDF<Vertex> pdf;
223  foreach (Vertex v, boost::vertices(g_))
224  {
225  const unsigned long int t = totalConnectionAttemptsProperty_[v];
226  pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
227  }
228 
229  if (pdf.empty())
230  return;
231 
232  while (ptc == false)
233  {
234  iterations_++;
235  Vertex v = pdf.sample(rng_.uniform01());
236  unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
237  if (s > 0)
238  {
239  s--;
240  Vertex last = addMilestone(si_->cloneState(workStates[s]));
241 
242  graphMutex_.lock();
243  for (unsigned int i = 0 ; i < s ; ++i)
244  {
245  // add the vertex along the bouncing motion
246  Vertex m = boost::add_vertex(g_);
247  stateProperty_[m] = si_->cloneState(workStates[i]);
248  totalConnectionAttemptsProperty_[m] = 1;
249  successfulConnectionAttemptsProperty_[m] = 0;
250  disjointSets_.make_set(m);
251 
252  // add the edge to the parent vertex
253  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
254  const Graph::edge_property_type properties(weight);
255  boost::add_edge(v, m, properties, g_);
256  uniteComponents(v, m);
257 
258  // add the vertex to the nearest neighbors data structure
259  nn_->add(m);
260  v = m;
261  }
262 
263  // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
264  // we add an edge
265  if (s > 0 || !sameComponent(v, last))
266  {
267  // add the edge to the parent vertex
268  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
269  const Graph::edge_property_type properties(weight);
270  boost::add_edge(v, last, properties, g_);
271  uniteComponents(v, last);
272  }
273  graphMutex_.unlock();
274  }
275  }
276 }
277 
279 {
280  growRoadmap(base::timedPlannerTerminationCondition(growTime));
281 }
282 
284 {
285  if (!isSetup())
286  setup();
287  if (!sampler_)
288  sampler_ = si_->allocValidStateSampler();
289 
290  base::State *workState = si_->allocState();
291  growRoadmap (ptc, workState);
292  si_->freeState(workState);
293 }
294 
296  base::State *workState)
297 {
298  /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
299  while (ptc == false)
300  {
301  iterations_++;
302  // search for a valid state
303  bool found = false;
304  while (!found && ptc == false)
305  {
306  unsigned int attempts = 0;
307  do
308  {
309  found = sampler_->sample(workState);
310  attempts++;
311  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
312  }
313  // add it as a milestone
314  if (found)
315  addMilestone(si_->cloneState(workState));
316  }
317 }
318 
320  base::PathPtr &solution)
321 {
322  base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
323  while (!ptc && !addedNewSolution_)
324  {
325  // Check for any new goal states
326  if (goal->maxSampleCount() > goalM_.size())
327  {
328  const base::State *st = pis_.nextGoal();
329  if (st)
330  goalM_.push_back(addMilestone(si_->cloneState(st)));
331  }
332 
333  // Check for a solution
334  addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution);
335  // Sleep for 1ms
336  if (!addedNewSolution_)
337  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
338  }
339 }
340 
341 bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution)
342 {
343  base::Goal *g = pdef_->getGoal().get();
344  base::Cost sol_cost(opt_->infiniteCost());
345  foreach (Vertex start, starts)
346  {
347  foreach (Vertex goal, goals)
348  {
349  // we lock because the connected components algorithm is incremental and may change disjointSets_
350  graphMutex_.lock();
351  bool same_component = sameComponent(start, goal);
352  graphMutex_.unlock();
353 
354  if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
355  {
356  base::PathPtr p = constructSolution(start, goal);
357  if (p)
358  {
359  base::Cost pathCost = p->cost(opt_);
360  if (opt_->isCostBetterThan(pathCost, bestCost_))
361  bestCost_ = pathCost;
362  // Check if optimization objective is satisfied
363  if (opt_->isSatisfied(pathCost))
364  {
365  solution = p;
366  return true;
367  }
368  else if (opt_->isCostBetterThan(pathCost, sol_cost))
369  {
370  solution = p;
371  sol_cost = pathCost;
372  }
373  }
374  }
375  }
376  }
377 
378  return false;
379 }
380 
382 {
383  return addedNewSolution_;
384 }
385 
387 {
388  checkValidity();
389  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
390 
391  if (!goal)
392  {
393  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
395  }
396 
397  // Add the valid start states as milestones
398  while (const base::State *st = pis_.nextStart())
399  startM_.push_back(addMilestone(si_->cloneState(st)));
400 
401  if (startM_.size() == 0)
402  {
403  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
405  }
406 
407  if (!goal->couldSample())
408  {
409  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
411  }
412 
413  // Ensure there is at least one valid goal state
414  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
415  {
416  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
417  if (st)
418  goalM_.push_back(addMilestone(si_->cloneState(st)));
419 
420  if (goalM_.empty())
421  {
422  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
424  }
425  }
426 
427  unsigned long int nrStartStates = boost::num_vertices(g_);
428  OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
429 
430  // Reset addedNewSolution_ member and create solution checking thread
431  addedNewSolution_ = false;
432  base::PathPtr sol;
433  boost::thread slnThread(boost::bind(&PRM::checkForSolution, this, ptc, boost::ref(sol)));
434 
435  // construct new planner termination condition that fires when the given ptc is true, or a solution is found
436  base::PlannerTerminationCondition ptcOrSolutionFound =
438 
439  constructRoadmap(ptcOrSolutionFound);
440 
441  // Ensure slnThread is ceased before exiting solve
442  slnThread.join();
443 
444  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
445 
446  if (sol)
447  {
448  base::PlannerSolution psol(sol);
449  psol.setPlannerName(getName());
450  // if the solution was optimized, we mark it as such
451  psol.setOptimized(opt_, bestCost_, addedNewSolution());
452  pdef_->addSolutionPath(psol);
453  }
454 
456 }
457 
459 {
460  if (!isSetup())
461  setup();
462  if (!sampler_)
463  sampler_ = si_->allocValidStateSampler();
464  if (!simpleSampler_)
465  simpleSampler_ = si_->allocStateSampler();
466 
467  std::vector<base::State*> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
468  si_->allocStates(xstates);
469  bool grow = true;
470 
471  bestCost_ = opt_->infiniteCost();
472  while (ptc() == false)
473  {
474  // maintain a 2:1 ratio for growing/expansion of roadmap
475  // call growRoadmap() twice as long for every call of expandRoadmap()
476  if (grow)
478  else
480  grow = !grow;
481  }
482 
483  si_->freeStates(xstates);
484 }
485 
487 {
488  boost::mutex::scoped_lock _(graphMutex_);
489 
490  Vertex m = boost::add_vertex(g_);
491  stateProperty_[m] = state;
492  totalConnectionAttemptsProperty_[m] = 1;
493  successfulConnectionAttemptsProperty_[m] = 0;
494 
495  // Initialize to its own (dis)connected component.
496  disjointSets_.make_set(m);
497 
498  // Which milestones will we attempt to connect to?
499  const std::vector<Vertex>& neighbors = connectionStrategy_(m);
500 
501  foreach (Vertex n, neighbors)
502  if (connectionFilter_(n, m))
503  {
504  totalConnectionAttemptsProperty_[m]++;
505  totalConnectionAttemptsProperty_[n]++;
506  if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
507  {
508  successfulConnectionAttemptsProperty_[m]++;
509  successfulConnectionAttemptsProperty_[n]++;
510  const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
511  const Graph::edge_property_type properties(weight);
512  boost::add_edge(n, m, properties, g_);
513  uniteComponents(n, m);
514  }
515  }
516 
517  nn_->add(m);
518 
519  return m;
520 }
521 
523 {
524  disjointSets_.union_set(m1, m2);
525 }
526 
528 {
529  return boost::same_component(m1, m2, disjointSets_);
530 }
531 
533 {
534  boost::mutex::scoped_lock _(graphMutex_);
535  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
536 
537  try
538  {
539  // Consider using a persistent distance_map if it's slow
540  boost::astar_search(g_, start,
541  boost::bind(&PRM::costHeuristic, this, _1, goal),
542  boost::predecessor_map(prev).
543  distance_compare(boost::bind(&base::OptimizationObjective::
544  isCostBetterThan, opt_.get(), _1, _2)).
545  distance_combine(boost::bind(&base::OptimizationObjective::
546  combineCosts, opt_.get(), _1, _2)).
547  distance_inf(opt_->infiniteCost()).
548  distance_zero(opt_->identityCost()).
549  visitor(AStarGoalVisitor<Vertex>(goal)));
550  }
551  catch (AStarFoundGoal&)
552  {
553  }
554 
555  if (prev[goal] == goal)
556  throw Exception(name_, "Could not find solution path");
557 
558  PathGeometric *p = new PathGeometric(si_);
559  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
560  p->append(stateProperty_[pos]);
561  p->append(stateProperty_[start]);
562  p->reverse();
563 
564  return base::PathPtr(p);
565 }
566 
568 {
569  Planner::getPlannerData(data);
570 
571  // Explicitly add start and goal states:
572  for (size_t i = 0; i < startM_.size(); ++i)
573  data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], const_cast<PRM*>(this)->disjointSets_.find_set(startM_[i])));
574 
575  for (size_t i = 0; i < goalM_.size(); ++i)
576  data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], const_cast<PRM*>(this)->disjointSets_.find_set(goalM_[i])));
577 
578  // Adding edges and all other vertices simultaneously
579  foreach(const Edge e, boost::edges(g_))
580  {
581  const Vertex v1 = boost::source(e, g_);
582  const Vertex v2 = boost::target(e, g_);
583  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]),
584  base::PlannerDataVertex(stateProperty_[v2]));
585 
586  // Add the reverse edge, since we're constructing an undirected roadmap
587  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]),
588  base::PlannerDataVertex(stateProperty_[v1]));
589 
590  // Add tags for the newly added vertices
591  data.tagState(stateProperty_[v1], const_cast<PRM*>(this)->disjointSets_.find_set(v1));
592  data.tagState(stateProperty_[v2], const_cast<PRM*>(this)->disjointSets_.find_set(v2));
593  }
594 }
595 
597 {
598  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
599 }
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:73
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
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:391
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
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:170
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:319
A boost 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:208
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:317
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition: PRM.cpp:198
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:527
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:63
static const double ROADMAP_BUILD_TIME
The time in seconds for a single roadmap building operation (dt)
Definition: PRM.cpp:65
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:278
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PRM.cpp:177
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
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:386
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
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:127
Abstract definition of a goal region that can be sampled.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
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:69
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:191
#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:486
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:532
The planner found an exact solution.
Definition: PlannerStatus.h:66
void reverse()
Reverse the path.
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...
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
A boost shared pointer wrapper for ompl::base::SpaceInformation.
An optimization objective which corresponds to optimizing path length.
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:50
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 ...
Abstract definition of optimization objectives.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
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:47
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
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.
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:522
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
Definition: PRM.cpp:149
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PRM.cpp:108
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: PRM.cpp:164
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:218
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:62
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:341
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
Definition: PRM.h:259
Definition of a geometric path.
Definition: PathGeometric.h:60
bool empty() const
Returns whether the PDF contains no data.
Definition: PDF.h:262
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: PRM.h:125
_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 constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:458
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 boost shared pointer wrapper for ompl::base::Path.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: PRM.cpp:567
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:381
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:596
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:55
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68