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