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 }
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:392
double getRange() const
Get the range the planner is using.
Definition: LazyPRM.h:190
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LazyPRM.cpp:256
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.
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:286
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 multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:263
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:382
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:114
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:474
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:124
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: LazyPRM.h:376
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:302
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:112
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:162
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
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:239
boost::property_map< Graph, edge_flags_t >::type edgeValidityProperty_
Access the validity state of an edge.
Definition: LazyPRM.h:388
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:493
@ INVALID_GOAL
Invalid goal state.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:270
Lazy Probabilistic RoadMap planner.
Definition: LazyPRM.h:106
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
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:458
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
@ EXACT_SOLUTION
The planner found an exact solution.
Graph g_
Connectivity graph.
Definition: LazyPRM.h:364
void freeMemory()
Free all the memory allocated by the planner.
Definition: LazyPRM.cpp:269
boost::property_map< Graph, vertex_flags_t >::type vertexValidityProperty_
Access the validity state of a vertex.
Definition: LazyPRM.h:385
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:332
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:481
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:361
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:241
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:266
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
boost::adjacency_list_traits< boost::vecS, boost::listS, boost::undirectedS >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: LazyPRM.h:130
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:79
@ INVALID_START
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:608
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:123
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
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:260
void clearValidity()
change the validity flag of each node and edge to VALIDITY_UNKNOWN
Definition: LazyPRM.cpp:248
Main namespace. Contains everything in this library.
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: LazyPRM.h:342