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 (!nn_)
230  {
231  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
232  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
233  {
234  return distanceFunction(a, b);
235  });
236  }
237 
238  if (starStrategy_)
239  connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
240  else
241  connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
242 }
243 
244 void ompl::geometric::LazyPRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
245 {
246  Planner::setProblemDefinition(pdef);
247  clearQuery();
248 }
249 
251 {
252  startM_.clear();
253  goalM_.clear();
254  pis_.restart();
255 }
256 
258 {
259  foreach (const Vertex v, boost::vertices(g_))
260  vertexValidityProperty_[v] = VALIDITY_UNKNOWN;
261  foreach (const Edge e, boost::edges(g_))
262  edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
263 }
264 
266 {
267  Planner::clear();
268  freeMemory();
269  if (nn_)
270  nn_->clear();
271  clearQuery();
272 
273  componentCount_ = 0;
274  iterations_ = 0;
275  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
276 }
277 
279 {
280  foreach (Vertex v, boost::vertices(g_))
281  si_->freeState(stateProperty_[v]);
282  g_.clear();
283 }
284 
286 {
287  Vertex m = boost::add_vertex(g_);
288  stateProperty_[m] = state;
289  vertexValidityProperty_[m] = VALIDITY_UNKNOWN;
290  unsigned long int newComponent = componentCount_++;
291  vertexComponentProperty_[m] = newComponent;
292  componentSize_[newComponent] = 1;
293 
294  // Which milestones will we attempt to connect to?
295  const std::vector<Vertex> &neighbors = connectionStrategy_(m);
296  foreach (Vertex n, neighbors)
297  if (connectionFilter_(m, n))
298  {
299  const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
300  const Graph::edge_property_type properties(weight);
301  const Edge &e = boost::add_edge(m, n, properties, g_).first;
302  edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
303  uniteComponents(m, n);
304  }
305 
306  nn_->add(m);
307 
308  return m;
309 }
310 
312 {
313  checkValidity();
314  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
315 
316  if (goal == nullptr)
317  {
318  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
320  }
321 
322  // Add the valid start states as milestones
323  while (const base::State *st = pis_.nextStart())
324  startM_.push_back(addMilestone(si_->cloneState(st)));
325 
326  if (startM_.empty())
327  {
328  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
330  }
331 
332  if (!goal->couldSample())
333  {
334  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
336  }
337 
338  // Ensure there is at least one valid goal state
339  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
340  {
341  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
342  if (st != nullptr)
343  goalM_.push_back(addMilestone(si_->cloneState(st)));
344 
345  if (goalM_.empty())
346  {
347  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
349  }
350  }
351 
352  unsigned long int nrStartStates = boost::num_vertices(g_);
353  OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
354 
355  bestCost_ = opt_->infiniteCost();
356  base::State *workState = si_->allocState();
357  std::pair<std::size_t, std::size_t> startGoalPair;
358  base::PathPtr bestSolution;
359  bool fullyOptimized = false;
360  bool someSolutionFound = false;
361  unsigned int optimizingComponentSegments = 0;
362 
363  // Grow roadmap in lazy fashion -- add vertices and edges without checking validity
364  while (!ptc)
365  {
366  ++iterations_;
367  sampler_->sampleUniform(workState);
368  Vertex addedVertex = addMilestone(si_->cloneState(workState));
369 
370  const long int solComponent = solutionComponent(&startGoalPair);
371  // If the start & goal are connected and we either did not find any solution
372  // so far or the one we found still needs optimizing and we just added an edge
373  // to the connected component that is used for the solution, we attempt to
374  // construct a new solution.
375  if (solComponent != -1 &&
376  (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent))
377  {
378  // If we already have a solution, we are optimizing. We check that we added at least
379  // a few segments to the connected component that includes the previously found
380  // solution before attempting to construct a new solution.
381  if (someSolutionFound)
382  {
383  if (++optimizingComponentSegments < magic::MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION)
384  continue;
385  optimizingComponentSegments = 0;
386  }
387  Vertex startV = startM_[startGoalPair.first];
388  Vertex goalV = goalM_[startGoalPair.second];
389  base::PathPtr solution;
390  do
391  {
392  solution = constructSolution(startV, goalV);
393  } while (!solution && vertexComponentProperty_[startV] == vertexComponentProperty_[goalV]);
394  if (solution)
395  {
396  someSolutionFound = true;
397  base::Cost c = solution->cost(opt_);
398  if (opt_->isSatisfied(c))
399  {
400  fullyOptimized = true;
401  bestSolution = solution;
402  bestCost_ = c;
403  break;
404  }
405  if (opt_->isCostBetterThan(c, bestCost_))
406  {
407  bestSolution = solution;
408  bestCost_ = c;
409  }
410  }
411  }
412  }
413 
414  si_->freeState(workState);
415 
416  if (bestSolution)
417  {
418  base::PlannerSolution psol(bestSolution);
419  psol.setPlannerName(getName());
420  // if the solution was optimized, we mark it as such
421  psol.setOptimized(opt_, bestCost_, fullyOptimized);
422  pdef_->addSolutionPath(psol);
423  }
424 
425  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
426 
428 }
429 
430 void ompl::geometric::LazyPRM::uniteComponents(Vertex a, Vertex b)
431 {
432  unsigned long int componentA = vertexComponentProperty_[a];
433  unsigned long int componentB = vertexComponentProperty_[b];
434  if (componentA == componentB)
435  return;
436  if (componentSize_[componentA] > componentSize_[componentB])
437  {
438  std::swap(componentA, componentB);
439  std::swap(a, b);
440  }
441  markComponent(a, componentB);
442 }
443 
444 void ompl::geometric::LazyPRM::markComponent(Vertex v, unsigned long int newComponent)
445 {
446  std::queue<Vertex> q;
447  q.push(v);
448  while (!q.empty())
449  {
450  Vertex n = q.front();
451  q.pop();
452  unsigned long int &component = vertexComponentProperty_[n];
453  if (component == newComponent)
454  continue;
455  if (componentSize_[component] == 1)
456  componentSize_.erase(component);
457  else
458  componentSize_[component]--;
459  component = newComponent;
460  componentSize_[newComponent]++;
461  boost::graph_traits<Graph>::adjacency_iterator nbh, last;
462  for (boost::tie(nbh, last) = boost::adjacent_vertices(n, g_); nbh != last; ++nbh)
463  q.push(*nbh);
464  }
465 }
466 
467 long int ompl::geometric::LazyPRM::solutionComponent(std::pair<std::size_t, std::size_t> *startGoalPair) const
468 {
469  for (std::size_t startIndex = 0; startIndex < startM_.size(); ++startIndex)
470  {
471  long int startComponent = vertexComponentProperty_[startM_[startIndex]];
472  for (std::size_t goalIndex = 0; goalIndex < goalM_.size(); ++goalIndex)
473  {
474  if (startComponent == (long int)vertexComponentProperty_[goalM_[goalIndex]])
475  {
476  startGoalPair->first = startIndex;
477  startGoalPair->second = goalIndex;
478  return startComponent;
479  }
480  }
481  }
482  return -1;
483 }
484 
486 {
487  // Need to update the index map here, becuse nodes may have been removed and
488  // the numbering will not be 0 .. N-1 otherwise.
489  unsigned long int index = 0;
490  boost::graph_traits<Graph>::vertex_iterator vi, vend;
491  for (boost::tie(vi, vend) = boost::vertices(g_); vi != vend; ++vi, ++index)
492  indexProperty_[*vi] = index;
493 
494  boost::property_map<Graph, boost::vertex_predecessor_t>::type prev;
495  try
496  {
497  // Consider using a persistent distance_map if it's slow
498  boost::astar_search(g_, start,
499  [this, goal](Vertex v)
500  {
501  return costHeuristic(v, goal);
502  },
503  boost::predecessor_map(prev)
504  .distance_compare([this](base::Cost c1, base::Cost c2)
505  {
506  return opt_->isCostBetterThan(c1, c2);
507  })
508  .distance_combine([this](base::Cost c1, base::Cost c2)
509  {
510  return opt_->combineCosts(c1, c2);
511  })
512  .distance_inf(opt_->infiniteCost())
513  .distance_zero(opt_->identityCost())
514  .visitor(AStarGoalVisitor<Vertex>(goal)));
515  }
516  catch (AStarFoundGoal &)
517  {
518  }
519  if (prev[goal] == goal)
520  throw Exception(name_, "Could not find solution path");
521 
522  // First, get the solution states without copying them, and check them for validity.
523  // We do all the node validity checks for the vertices, as this may remove a larger
524  // part of the graph (compared to removing an edge).
525  std::vector<const base::State *> states(1, stateProperty_[goal]);
526  std::set<Vertex> milestonesToRemove;
527  for (Vertex pos = prev[goal]; prev[pos] != pos; pos = prev[pos])
528  {
529  const base::State *st = stateProperty_[pos];
530  unsigned int &vd = vertexValidityProperty_[pos];
531  if ((vd & VALIDITY_TRUE) == 0)
532  if (si_->isValid(st))
533  vd |= VALIDITY_TRUE;
534  if ((vd & VALIDITY_TRUE) == 0)
535  milestonesToRemove.insert(pos);
536  if (milestonesToRemove.empty())
537  states.push_back(st);
538  }
539 
540  // We remove *all* invalid vertices. This is not entirely as described in the original LazyPRM
541  // paper, as the paper suggest removing the first vertex only, and then recomputing the
542  // shortest path. Howeve, the paper says the focus is on efficient vertex & edge removal,
543  // rather than collision checking, so this modification is in the spirit of the paper.
544  if (!milestonesToRemove.empty())
545  {
546  unsigned long int comp = vertexComponentProperty_[start];
547  // Remember the current neighbors.
548  std::set<Vertex> neighbors;
549  for (auto it = milestonesToRemove.begin(); it != milestonesToRemove.end(); ++it)
550  {
551  boost::graph_traits<Graph>::adjacency_iterator nbh, last;
552  for (boost::tie(nbh, last) = boost::adjacent_vertices(*it, g_); nbh != last; ++nbh)
553  if (milestonesToRemove.find(*nbh) == milestonesToRemove.end())
554  neighbors.insert(*nbh);
555  // Remove vertex from nearest neighbors data structure.
556  nn_->remove(*it);
557  // Free vertex state.
558  si_->freeState(stateProperty_[*it]);
559  // Remove all edges.
560  boost::clear_vertex(*it, g_);
561  // Remove the vertex.
562  boost::remove_vertex(*it, g_);
563  }
564  // Update the connected component ID for neighbors.
565  for (auto neighbor : neighbors)
566  {
567  if (comp == vertexComponentProperty_[neighbor])
568  {
569  unsigned long int newComponent = componentCount_++;
570  componentSize_[newComponent] = 0;
571  markComponent(neighbor, newComponent);
572  }
573  }
574  return base::PathPtr();
575  }
576 
577  // start is checked for validity already
578  states.push_back(stateProperty_[start]);
579 
580  // Check the edges too, if the vertices were valid. Remove the first invalid edge only.
581  std::vector<const base::State *>::const_iterator prevState = states.begin(), state = prevState + 1;
582  Vertex prevVertex = goal, pos = prev[goal];
583  do
584  {
585  Edge e = boost::lookup_edge(pos, prevVertex, g_).first;
586  unsigned int &evd = edgeValidityProperty_[e];
587  if ((evd & VALIDITY_TRUE) == 0)
588  {
589  if (si_->checkMotion(*state, *prevState))
590  evd |= VALIDITY_TRUE;
591  }
592  if ((evd & VALIDITY_TRUE) == 0)
593  {
594  boost::remove_edge(e, g_);
595  unsigned long int newComponent = componentCount_++;
596  componentSize_[newComponent] = 0;
597  markComponent(pos, newComponent);
598  return base::PathPtr();
599  }
600  prevState = state;
601  ++state;
602  prevVertex = pos;
603  pos = prev[pos];
604  } while (prevVertex != pos);
605 
606  auto p(std::make_shared<PathGeometric>(si_));
607  for (std::vector<const base::State *>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
608  p->append(*st);
609  return p;
610 }
611 
613 {
614  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
615 }
616 
618 {
619  Planner::getPlannerData(data);
620 
621  // Explicitly add start and goal states. Tag all states known to be valid as 1.
622  // Unchecked states are tagged as 0.
623  for (auto i : startM_)
624  data.addStartVertex(base::PlannerDataVertex(stateProperty_[i], 1));
625 
626  for (auto i : goalM_)
627  data.addGoalVertex(base::PlannerDataVertex(stateProperty_[i], 1));
628 
629  // Adding edges and all other vertices simultaneously
630  foreach (const Edge e, boost::edges(g_))
631  {
632  const Vertex v1 = boost::source(e, g_);
633  const Vertex v2 = boost::target(e, g_);
634  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
635 
636  // Add the reverse edge, since we're constructing an undirected roadmap
637  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
638 
639  // Add tags for the newly added vertices
640  data.tagState(stateProperty_[v1], (vertexValidityProperty_[v1] & VALIDITY_TRUE) == 0 ? 0 : 1);
641  data.tagState(stateProperty_[v2], (vertexValidityProperty_[v2] & VALIDITY_TRUE) == 0 ? 0 : 1);
642  }
643 }
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:250
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:257
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:278
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:612
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:485
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LazyPRM.cpp:265
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:311
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:285
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:617
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:467
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.
Definition: AppBase.h:22
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