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 }
void setDefaultConnectionStrategy()
Definition: LazyPRM.cpp:227
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
double getRange() const
Get the range the planner is using.
Definition: LazyPRM.h:158
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LazyPRM.cpp:265
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...
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
A shared pointer wrapper for ompl::base::Path.
static const unsigned int VALIDITY_UNKNOWN
Flag indicating validity of an edge of a vertex.
Definition: LazyPRM.h:254
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 multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:199
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition: LazyPRM.cpp:209
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)....
boost::property_map< Graph, vertex_component_t >::type vertexComponentProperty_
Access the connected component of a vertex.
Definition: LazyPRM.h:350
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LazyPRM.cpp:153
Definition of an abstract state.
Definition: State.h:50
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:410
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: LazyPRM.h:344
void setRange(double distance)
Set the maximum length of a motion to be added to the roadmap.
Definition: LazyPRM.cpp:200
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
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:48
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:130
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62
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:175
boost::property_map< Graph, edge_flags_t >::type edgeValidityProperty_
Access the validity state of an edge.
Definition: LazyPRM.h:356
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:206
Lazy Probabilistic RoadMap planner.
Definition: LazyPRM.h:74
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
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
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
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:467
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
Graph g_
Connectivity graph.
Definition: LazyPRM.h:332
void freeMemory()
Free all the memory allocated by the planner.
Definition: LazyPRM.cpp:278
boost::property_map< Graph, vertex_flags_t >::type vertexValidityProperty_
Access the validity state of a vertex.
Definition: LazyPRM.h:353
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
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
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...
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: LazyPRM.h:329
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:64
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: LazyPRM.cpp:250
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
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
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
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:47
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
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
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
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
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:196
void clearValidity()
change the validity flag of each node and edge to VALIDITY_UNKNOWN
Definition: LazyPRM.cpp:257
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: LazyPRM.h:310