LazyPRM.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Willow Garage
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 Willow Garage 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, Ryan Luna */
36 
37 #include "ompl/geometric/planners/prm/LazyPRM.h"
38 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
41 #include "ompl/tools/config/SelfConfig.h"
42 #include <boost/graph/astar_search.hpp>
43 #include <boost/graph/incremental_components.hpp>
44 #include <boost/graph/lookup_edge.hpp>
45 #include <boost/foreach.hpp>
46 #include <queue>
47 
48 #include "GoalVisitor.hpp"
49 
50 #define foreach BOOST_FOREACH
51 
52 namespace ompl
53 {
54  namespace magic
55  {
58  static const unsigned int DEFAULT_NEAREST_NEIGHBORS_LAZY = 5;
59 
63  static const unsigned int MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION = 5;
64  }
65 }
66 
67 ompl::geometric::LazyPRM::LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy)
68  : base::Planner(si, "LazyPRM")
69  , starStrategy_(starStrategy)
70  , indexProperty_(boost::get(boost::vertex_index_t(), g_))
71  , stateProperty_(boost::get(vertex_state_t(), g_))
72  , weightProperty_(boost::get(boost::edge_weight, g_))
73  , vertexComponentProperty_(boost::get(vertex_component_t(), g_))
74  , vertexValidityProperty_(boost::get(vertex_flags_t(), g_))
75  , edgeValidityProperty_(boost::get(edge_flags_t(), g_))
76 {
79  specs_.optimizingPaths = true;
80 
81  Planner::declareParam<double>("range", this, &LazyPRM::setRange, &LazyPRM::getRange, "0.:1.:10000.");
82  if (!starStrategy_)
83  Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &LazyPRM::setMaxNearestNeighbors,
84  std::string("8:1000"));
85 
86  addPlannerProgressProperty("iterations INTEGER", [this]
87  {
88  return getIterationCount();
89  });
90  addPlannerProgressProperty("best cost REAL", [this]
91  {
92  return getBestCost();
93  });
94  addPlannerProgressProperty("milestone count INTEGER", [this]
95  {
96  return getMilestoneCountString();
97  });
98  addPlannerProgressProperty("edge count INTEGER", [this]
99  {
100  return getEdgeCountString();
101  });
102 }
103 
104 ompl::geometric::LazyPRM::~LazyPRM() = default;
105 
107 {
108  Planner::setup();
109  tools::SelfConfig sc(si_, getName());
110  sc.configurePlannerRange(maxDistance_);
111 
112  if (!nn_)
113  {
114  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
115  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
116  {
117  return distanceFunction(a, b);
118  });
119  }
120  if (!connectionStrategy_)
121  setDefaultConnectionStrategy();
122  if (!connectionFilter_)
123  connectionFilter_ = [](const Vertex &, const Vertex &)
124  {
125  return true;
126  };
127 
128  // Setup optimization objective
129  //
130  // If no optimization objective was specified, then default to
131  // optimizing path length as computed by the distance() function
132  // in the state space.
133  if (pdef_)
134  {
135  if (pdef_->hasOptimizationObjective())
136  opt_ = pdef_->getOptimizationObjective();
137  else
138  {
139  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
140  if (!starStrategy_)
141  opt_->setCostThreshold(opt_->infiniteCost());
142  }
143  }
144  else
145  {
146  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
147  setup_ = false;
148  }
149 
150  sampler_ = si_->allocStateSampler();
151 }
152 
154 {
155  maxDistance_ = distance;
156  if (!userSetConnectionStrategy_)
157  setDefaultConnectionStrategy();
158  if (isSetup())
159  setup();
160 }
161 
163 {
164  if (starStrategy_)
165  throw Exception("Cannot set the maximum nearest neighbors for " + getName());
166  if (!nn_)
167  {
168  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
169  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
170  {
171  return distanceFunction(a, b);
172  });
173  }
174  if (!userSetConnectionStrategy_)
175  connectionStrategy_ = KBoundedStrategy<Vertex>(k, maxDistance_, nn_);
176  if (isSetup())
177  setup();
178 }
179 
181 {
182  if (starStrategy_)
183  connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
184  else
185  connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
186 }
187 
188 void ompl::geometric::LazyPRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
189 {
190  Planner::setProblemDefinition(pdef);
191  clearQuery();
192 }
193 
195 {
196  startM_.clear();
197  goalM_.clear();
198  pis_.restart();
199 }
200 
202 {
203  Planner::clear();
204  freeMemory();
205  if (nn_)
206  nn_->clear();
207  clearQuery();
208 
209  componentCount_ = 0;
210  iterations_ = 0;
211  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
212 }
213 
215 {
216  foreach (Vertex v, boost::vertices(g_))
217  si_->freeState(stateProperty_[v]);
218  g_.clear();
219 }
220 
222 {
223  Vertex m = boost::add_vertex(g_);
224  stateProperty_[m] = state;
225  vertexValidityProperty_[m] = VALIDITY_UNKNOWN;
226  unsigned long int newComponent = componentCount_++;
227  vertexComponentProperty_[m] = newComponent;
228  componentSize_[newComponent] = 1;
229 
230  // Which milestones will we attempt to connect to?
231  const std::vector<Vertex> &neighbors = connectionStrategy_(m);
232  foreach (Vertex n, neighbors)
233  if (connectionFilter_(m, n))
234  {
235  const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
236  const Graph::edge_property_type properties(weight);
237  const Edge &e = boost::add_edge(m, n, properties, g_).first;
238  edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
239  uniteComponents(m, n);
240  }
241 
242  nn_->add(m);
243 
244  return m;
245 }
246 
248 {
249  checkValidity();
250  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
251 
252  if (goal == nullptr)
253  {
254  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
256  }
257 
258  // Add the valid start states as milestones
259  while (const base::State *st = pis_.nextStart())
260  startM_.push_back(addMilestone(si_->cloneState(st)));
261 
262  if (startM_.empty())
263  {
264  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
266  }
267 
268  if (!goal->couldSample())
269  {
270  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
272  }
273 
274  // Ensure there is at least one valid goal state
275  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
276  {
277  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
278  if (st != nullptr)
279  goalM_.push_back(addMilestone(si_->cloneState(st)));
280 
281  if (goalM_.empty())
282  {
283  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
285  }
286  }
287 
288  unsigned long int nrStartStates = boost::num_vertices(g_);
289  OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
290 
291  bestCost_ = opt_->infiniteCost();
292  base::State *workState = si_->allocState();
293  std::pair<std::size_t, std::size_t> startGoalPair;
294  base::PathPtr bestSolution;
295  bool fullyOptimized = false;
296  bool someSolutionFound = false;
297  unsigned int optimizingComponentSegments = 0;
298 
299  // Grow roadmap in lazy fashion -- add vertices and edges without checking validity
300  while (!ptc)
301  {
302  ++iterations_;
303  sampler_->sampleUniform(workState);
304  Vertex addedVertex = addMilestone(si_->cloneState(workState));
305 
306  const long int solComponent = solutionComponent(&startGoalPair);
307  // If the start & goal are connected and we either did not find any solution
308  // so far or the one we found still needs optimizing and we just added an edge
309  // to the connected component that is used for the solution, we attempt to
310  // construct a new solution.
311  if (solComponent != -1 &&
312  (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent))
313  {
314  // If we already have a solution, we are optimizing. We check that we added at least
315  // a few segments to the connected component that includes the previously found
316  // solution before attempting to construct a new solution.
317  if (someSolutionFound)
318  {
319  if (++optimizingComponentSegments < magic::MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION)
320  continue;
321  optimizingComponentSegments = 0;
322  }
323  Vertex startV = startM_[startGoalPair.first];
324  Vertex goalV = goalM_[startGoalPair.second];
325  base::PathPtr solution;
326  do
327  {
328  solution = constructSolution(startV, goalV);
329  } while (!solution && vertexComponentProperty_[startV] == vertexComponentProperty_[goalV]);
330  if (solution)
331  {
332  someSolutionFound = true;
333  base::Cost c = solution->cost(opt_);
334  if (opt_->isSatisfied(c))
335  {
336  fullyOptimized = true;
337  bestSolution = solution;
338  bestCost_ = c;
339  break;
340  }
341  if (opt_->isCostBetterThan(c, bestCost_))
342  {
343  bestSolution = solution;
344  bestCost_ = c;
345  }
346  }
347  }
348  }
349 
350  si_->freeState(workState);
351 
352  if (bestSolution)
353  {
354  base::PlannerSolution psol(bestSolution);
355  psol.setPlannerName(getName());
356  // if the solution was optimized, we mark it as such
357  psol.setOptimized(opt_, bestCost_, fullyOptimized);
358  pdef_->addSolutionPath(psol);
359  }
360 
361  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
362 
364 }
365 
366 void ompl::geometric::LazyPRM::uniteComponents(Vertex a, Vertex b)
367 {
368  unsigned long int componentA = vertexComponentProperty_[a];
369  unsigned long int componentB = vertexComponentProperty_[b];
370  if (componentA == componentB)
371  return;
372  if (componentSize_[componentA] > componentSize_[componentB])
373  {
374  std::swap(componentA, componentB);
375  std::swap(a, b);
376  }
377  markComponent(a, componentB);
378 }
379 
380 void ompl::geometric::LazyPRM::markComponent(Vertex v, unsigned long int newComponent)
381 {
382  std::queue<Vertex> q;
383  q.push(v);
384  while (!q.empty())
385  {
386  Vertex n = q.front();
387  q.pop();
388  unsigned long int &component = vertexComponentProperty_[n];
389  if (component == newComponent)
390  continue;
391  if (componentSize_[component] == 1)
392  componentSize_.erase(component);
393  else
394  componentSize_[component]--;
395  component = newComponent;
396  componentSize_[newComponent]++;
397  boost::graph_traits<Graph>::adjacency_iterator nbh, last;
398  for (boost::tie(nbh, last) = boost::adjacent_vertices(n, g_); nbh != last; ++nbh)
399  q.push(*nbh);
400  }
401 }
402 
403 long int ompl::geometric::LazyPRM::solutionComponent(std::pair<std::size_t, std::size_t> *startGoalPair) const
404 {
405  for (std::size_t startIndex = 0; startIndex < startM_.size(); ++startIndex)
406  {
407  long int startComponent = vertexComponentProperty_[startM_[startIndex]];
408  for (std::size_t goalIndex = 0; goalIndex < goalM_.size(); ++goalIndex)
409  {
410  if (startComponent == (long int)vertexComponentProperty_[goalM_[goalIndex]])
411  {
412  startGoalPair->first = startIndex;
413  startGoalPair->second = goalIndex;
414  return startComponent;
415  }
416  }
417  }
418  return -1;
419 }
420 
422 {
423  // Need to update the index map here, becuse nodes may have been removed and
424  // the numbering will not be 0 .. N-1 otherwise.
425  unsigned long int index = 0;
426  boost::graph_traits<Graph>::vertex_iterator vi, vend;
427  for (boost::tie(vi, vend) = boost::vertices(g_); vi != vend; ++vi, ++index)
428  indexProperty_[*vi] = index;
429 
430  boost::property_map<Graph, boost::vertex_predecessor_t>::type prev;
431  try
432  {
433  // Consider using a persistent distance_map if it's slow
434  boost::astar_search(g_, start,
435  [this, goal](Vertex v)
436  {
437  return costHeuristic(v, goal);
438  },
439  boost::predecessor_map(prev)
440  .distance_compare([this](base::Cost c1, base::Cost c2)
441  {
442  return opt_->isCostBetterThan(c1, c2);
443  })
444  .distance_combine([this](base::Cost c1, base::Cost c2)
445  {
446  return opt_->combineCosts(c1, c2);
447  })
448  .distance_inf(opt_->infiniteCost())
449  .distance_zero(opt_->identityCost())
450  .visitor(AStarGoalVisitor<Vertex>(goal)));
451  }
452  catch (AStarFoundGoal &)
453  {
454  }
455  if (prev[goal] == goal)
456  throw Exception(name_, "Could not find solution path");
457 
458  // First, get the solution states without copying them, and check them for validity.
459  // We do all the node validity checks for the vertices, as this may remove a larger
460  // part of the graph (compared to removing an edge).
461  std::vector<const base::State *> states(1, stateProperty_[goal]);
462  std::set<Vertex> milestonesToRemove;
463  for (Vertex pos = prev[goal]; prev[pos] != pos; pos = prev[pos])
464  {
465  const base::State *st = stateProperty_[pos];
466  unsigned int &vd = vertexValidityProperty_[pos];
467  if ((vd & VALIDITY_TRUE) == 0)
468  if (si_->isValid(st))
469  vd |= VALIDITY_TRUE;
470  if ((vd & VALIDITY_TRUE) == 0)
471  milestonesToRemove.insert(pos);
472  if (milestonesToRemove.empty())
473  states.push_back(st);
474  }
475 
476  // We remove *all* invalid vertices. This is not entirely as described in the original LazyPRM
477  // paper, as the paper suggest removing the first vertex only, and then recomputing the
478  // shortest path. Howeve, the paper says the focus is on efficient vertex & edge removal,
479  // rather than collision checking, so this modification is in the spirit of the paper.
480  if (!milestonesToRemove.empty())
481  {
482  unsigned long int comp = vertexComponentProperty_[start];
483  // Remember the current neighbors.
484  std::set<Vertex> neighbors;
485  for (auto it = milestonesToRemove.begin(); it != milestonesToRemove.end(); ++it)
486  {
487  boost::graph_traits<Graph>::adjacency_iterator nbh, last;
488  for (boost::tie(nbh, last) = boost::adjacent_vertices(*it, g_); nbh != last; ++nbh)
489  if (milestonesToRemove.find(*nbh) == milestonesToRemove.end())
490  neighbors.insert(*nbh);
491  // Remove vertex from nearest neighbors data structure.
492  nn_->remove(*it);
493  // Free vertex state.
494  si_->freeState(stateProperty_[*it]);
495  // Remove all edges.
496  boost::clear_vertex(*it, g_);
497  // Remove the vertex.
498  boost::remove_vertex(*it, g_);
499  }
500  // Update the connected component ID for neighbors.
501  for (auto neighbor : neighbors)
502  {
503  if (comp == vertexComponentProperty_[neighbor])
504  {
505  unsigned long int newComponent = componentCount_++;
506  componentSize_[newComponent] = 0;
507  markComponent(neighbor, newComponent);
508  }
509  }
510  return base::PathPtr();
511  }
512 
513  // start is checked for validity already
514  states.push_back(stateProperty_[start]);
515 
516  // Check the edges too, if the vertices were valid. Remove the first invalid edge only.
517  std::vector<const base::State *>::const_iterator prevState = states.begin(), state = prevState + 1;
518  Vertex prevVertex = goal, pos = prev[goal];
519  do
520  {
521  Edge e = boost::lookup_edge(pos, prevVertex, g_).first;
522  unsigned int &evd = edgeValidityProperty_[e];
523  if ((evd & VALIDITY_TRUE) == 0)
524  {
525  if (si_->checkMotion(*state, *prevState))
526  evd |= VALIDITY_TRUE;
527  }
528  if ((evd & VALIDITY_TRUE) == 0)
529  {
530  boost::remove_edge(e, g_);
531  unsigned long int newComponent = componentCount_++;
532  componentSize_[newComponent] = 0;
533  markComponent(pos, newComponent);
534  return base::PathPtr();
535  }
536  prevState = state;
537  ++state;
538  prevVertex = pos;
539  pos = prev[pos];
540  } while (prevVertex != pos);
541 
542  auto p(std::make_shared<PathGeometric>(si_));
543  for (std::vector<const base::State *>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
544  p->append(*st);
545  return p;
546 }
547 
549 {
550  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
551 }
552 
554 {
555  Planner::getPlannerData(data);
556 
557  // Explicitly add start and goal states. Tag all states known to be valid as 1.
558  // Unchecked states are tagged as 0.
559  for (auto i : startM_)
560  data.addStartVertex(base::PlannerDataVertex(stateProperty_[i], 1));
561 
562  for (auto i : goalM_)
563  data.addGoalVertex(base::PlannerDataVertex(stateProperty_[i], 1));
564 
565  // Adding edges and all other vertices simultaneously
566  foreach (const Edge e, boost::edges(g_))
567  {
568  const Vertex v1 = boost::source(e, g_);
569  const Vertex v2 = boost::target(e, g_);
570  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
571 
572  // Add the reverse edge, since we're constructing an undirected roadmap
573  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
574 
575  // Add tags for the newly added vertices
576  data.tagState(stateProperty_[v1], (vertexValidityProperty_[v1] & VALIDITY_TRUE) == 0 ? 0 : 1);
577  data.tagState(stateProperty_[v2], (vertexValidityProperty_[v2] & VALIDITY_TRUE) == 0 ? 0 : 1);
578  }
579 }
void setDefaultConnectionStrategy()
Definition: LazyPRM.cpp:180
double getRange() const
Get the range the planner is using.
Definition: LazyPRM.h:185
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LazyPRM.cpp:201
The goal is of a type that a planner does not recognize.
A shared pointer wrapper for ompl::base::Path.
Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition: LazyPRM.cpp:221
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:224
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition: LazyPRM.cpp:162
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LazyPRM.cpp:106
Definition of an abstract state.
Definition: State.h:110
LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: LazyPRM.cpp:67
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:472
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:120
void setRange(double distance)
Set the maximum length of a motion to be added to the roadmap.
Definition: LazyPRM.cpp:153
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: LazyPRM.cpp:247
Representation of a solution to a planning problem.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:108
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,...
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition: LazyPRM.h:160
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:67
The planner failed to find a solution.
Make the minimal number of connections required to ensure asymptotic optimality.
static const unsigned int MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION
When optimizing solutions with lazy planners, this is the minimum number of path segments to add befo...
Definition: LazyPRM.cpp:63
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:235
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:491
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:268
A class to store the exit status of Planner::solve()
static const unsigned int DEFAULT_NEAREST_NEIGHBORS_LAZY
The number of nearest neighbors to consider by default in the construction of the PRM roadmap.
Definition: LazyPRM.cpp:58
Return at most k neighbors, as long as they are also within a specified bound.
long int solutionComponent(std::pair< std::size_t, std::size_t > *startGoalPair) const
Check if any pair of a start state and goal state are part of the same connected component....
Definition: LazyPRM.cpp:403
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:148
The planner found an exact solution.
void freeMemory()
Free all the memory allocated by the planner.
Definition: LazyPRM.cpp:214
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...
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...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:63
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: LazyPRM.cpp:194
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:264
ompl::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: LazyPRM.cpp:421
boost::adjacency_list_traits< boost::vecS, boost::listS, boost::undirectedS >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: LazyPRM.h:128
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
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...
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 a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:76
Invalid start state or no start state specified.
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: LazyPRM.cpp:553
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:119
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: LazyPRM.cpp:548
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:258
Main namespace. Contains everything in this library.
Definition: AppBase.h:20
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: LazyPRM.h:334