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