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