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