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  , userSetConnectionStrategy_(false)
71  , maxDistance_(0.0)
72  , indexProperty_(boost::get(boost::vertex_index_t(), g_))
73  , stateProperty_(boost::get(vertex_state_t(), g_))
74  , weightProperty_(boost::get(boost::edge_weight, g_))
75  , vertexComponentProperty_(boost::get(vertex_component_t(), g_))
76  , vertexValidityProperty_(boost::get(vertex_flags_t(), g_))
77  , edgeValidityProperty_(boost::get(edge_flags_t(), g_))
78  , componentCount_(0)
79  , bestCost_(std::numeric_limits<double>::quiet_NaN())
80  , iterations_(0)
81 {
84  specs_.optimizingPaths = true;
85 
86  Planner::declareParam<double>("range", this, &LazyPRM::setRange, &LazyPRM::getRange, "0.:1.:10000.");
87  if (!starStrategy_)
88  Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &LazyPRM::setMaxNearestNeighbors,
89  std::string("8:1000"));
90 
91  addPlannerProgressProperty("iterations INTEGER", [this]
92  {
93  return getIterationCount();
94  });
95  addPlannerProgressProperty("best cost REAL", [this]
96  {
97  return getBestCost();
98  });
99  addPlannerProgressProperty("milestone count INTEGER", [this]
100  {
101  return getMilestoneCountString();
102  });
103  addPlannerProgressProperty("edge count INTEGER", [this]
104  {
105  return getEdgeCountString();
106  });
107 }
108 
109 ompl::geometric::LazyPRM::~LazyPRM() = default;
110 
112 {
113  Planner::setup();
116 
117  if (!nn_)
118  {
119  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
120  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
121  {
122  return distanceFunction(a, b);
123  });
124  }
125  if (!connectionStrategy_)
126  {
127  if (starStrategy_)
129  [this]
130  {
131  return milestoneCount();
132  },
133  nn_, si_->getStateDimension());
134  else
136  }
137  if (!connectionFilter_)
138  connectionFilter_ = [](const Vertex &, const Vertex &)
139  {
140  return true;
141  };
142 
143  // Setup optimization objective
144  //
145  // If no optimization objective was specified, then default to
146  // optimizing path length as computed by the distance() function
147  // in the state space.
148  if (pdef_)
149  {
150  if (pdef_->hasOptimizationObjective())
151  opt_ = pdef_->getOptimizationObjective();
152  else
153  {
154  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
155  if (!starStrategy_)
156  opt_->setCostThreshold(opt_->infiniteCost());
157  }
158  }
159  else
160  {
161  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
162  setup_ = false;
163  }
164 
165  sampler_ = si_->allocStateSampler();
166 }
167 
169 {
170  maxDistance_ = distance;
173  if (isSetup())
174  setup();
175 }
176 
178 {
179  if (starStrategy_)
180  throw Exception("Cannot set the maximum nearest neighbors for " + getName());
181  if (!nn_)
182  {
183  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
184  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
185  {
186  return distanceFunction(a, b);
187  });
188  }
191  if (isSetup())
192  setup();
193 }
194 
196 {
197  Planner::setProblemDefinition(pdef);
198  clearQuery();
199 }
200 
202 {
203  startM_.clear();
204  goalM_.clear();
205  pis_.restart();
206 }
207 
209 {
210  Planner::clear();
211  freeMemory();
212  if (nn_)
213  nn_->clear();
214  clearQuery();
215 
216  componentCount_ = 0;
217  iterations_ = 0;
218  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
219 }
220 
222 {
223  foreach (Vertex v, boost::vertices(g_))
224  si_->freeState(stateProperty_[v]);
225  g_.clear();
226 }
227 
229 {
230  Vertex m = boost::add_vertex(g_);
231  stateProperty_[m] = state;
233  unsigned long int newComponent = componentCount_++;
234  vertexComponentProperty_[m] = newComponent;
235  componentSize_[newComponent] = 1;
236 
237  // Which milestones will we attempt to connect to?
238  const std::vector<Vertex> &neighbors = connectionStrategy_(m);
239  foreach (Vertex n, neighbors)
240  if (connectionFilter_(m, n))
241  {
242  const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
243  const Graph::edge_property_type properties(weight);
244  const Edge &e = boost::add_edge(m, n, properties, g_).first;
246  uniteComponents(m, n);
247  }
248 
249  nn_->add(m);
250 
251  return m;
252 }
253 
255 {
256  checkValidity();
257  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
258 
259  if (!goal)
260  {
261  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
263  }
264 
265  // Add the valid start states as milestones
266  while (const base::State *st = pis_.nextStart())
267  startM_.push_back(addMilestone(si_->cloneState(st)));
268 
269  if (startM_.size() == 0)
270  {
271  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
273  }
274 
275  if (!goal->couldSample())
276  {
277  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
279  }
280 
281  // Ensure there is at least one valid goal state
282  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
283  {
284  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
285  if (st)
286  goalM_.push_back(addMilestone(si_->cloneState(st)));
287 
288  if (goalM_.empty())
289  {
290  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
292  }
293  }
294 
295  unsigned long int nrStartStates = boost::num_vertices(g_);
296  OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
297 
298  bestCost_ = opt_->infiniteCost();
299  base::State *workState = si_->allocState();
300  std::pair<std::size_t, std::size_t> startGoalPair;
301  base::PathPtr bestSolution;
302  bool fullyOptimized = false;
303  bool someSolutionFound = false;
304  unsigned int optimizingComponentSegments = 0;
305 
306  // Grow roadmap in lazy fashion -- add vertices and edges without checking validity
307  while (ptc == false)
308  {
309  ++iterations_;
310  sampler_->sampleUniform(workState);
311  Vertex addedVertex = addMilestone(si_->cloneState(workState));
312 
313  const long int solComponent = solutionComponent(&startGoalPair);
314  // If the start & goal are connected and we either did not find any solution
315  // so far or the one we found still needs optimizing and we just added an edge
316  // to the connected component that is used for the solution, we attempt to
317  // construct a new solution.
318  if (solComponent != -1 &&
319  (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent))
320  {
321  // If we already have a solution, we are optimizing. We check that we added at least
322  // a few segments to the connected component that includes the previously found
323  // solution before attempting to construct a new solution.
324  if (someSolutionFound)
325  {
326  if (++optimizingComponentSegments < magic::MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION)
327  continue;
328  optimizingComponentSegments = 0;
329  }
330  Vertex startV = startM_[startGoalPair.first];
331  Vertex goalV = goalM_[startGoalPair.second];
332  base::PathPtr solution;
333  do
334  {
335  solution = constructSolution(startV, goalV);
336  } while (!solution && vertexComponentProperty_[startV] == vertexComponentProperty_[goalV]);
337  if (solution)
338  {
339  someSolutionFound = true;
340  base::Cost c = solution->cost(opt_);
341  if (opt_->isSatisfied(c))
342  {
343  fullyOptimized = true;
344  bestSolution = solution;
345  bestCost_ = c;
346  break;
347  }
348  else
349  {
350  if (opt_->isCostBetterThan(c, bestCost_))
351  {
352  bestSolution = solution;
353  bestCost_ = c;
354  }
355  }
356  }
357  }
358  }
359 
360  si_->freeState(workState);
361 
362  if (bestSolution)
363  {
364  base::PlannerSolution psol(bestSolution);
365  psol.setPlannerName(getName());
366  // if the solution was optimized, we mark it as such
367  psol.setOptimized(opt_, bestCost_, fullyOptimized);
368  pdef_->addSolutionPath(psol);
369  }
370 
371  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
372 
374 }
375 
376 void ompl::geometric::LazyPRM::uniteComponents(Vertex a, Vertex b)
377 {
378  unsigned long int componentA = vertexComponentProperty_[a];
379  unsigned long int componentB = vertexComponentProperty_[b];
380  if (componentA == componentB)
381  return;
382  if (componentSize_[componentA] > componentSize_[componentB])
383  {
384  std::swap(componentA, componentB);
385  std::swap(a, b);
386  }
387  markComponent(a, componentB);
388 }
389 
390 void ompl::geometric::LazyPRM::markComponent(Vertex v, unsigned long int newComponent)
391 {
392  std::queue<Vertex> q;
393  q.push(v);
394  while (!q.empty())
395  {
396  Vertex n = q.front();
397  q.pop();
398  unsigned long int &component = vertexComponentProperty_[n];
399  if (component == newComponent)
400  continue;
401  if (componentSize_[component] == 1)
402  componentSize_.erase(component);
403  else
404  componentSize_[component]--;
405  component = newComponent;
406  componentSize_[newComponent]++;
407  boost::graph_traits<Graph>::adjacency_iterator nbh, last;
408  for (boost::tie(nbh, last) = boost::adjacent_vertices(n, g_); nbh != last; ++nbh)
409  q.push(*nbh);
410  }
411 }
412 
413 long int ompl::geometric::LazyPRM::solutionComponent(std::pair<std::size_t, std::size_t> *startGoalPair) const
414 {
415  for (std::size_t startIndex = 0; startIndex < startM_.size(); ++startIndex)
416  {
417  long int startComponent = vertexComponentProperty_[startM_[startIndex]];
418  for (std::size_t goalIndex = 0; goalIndex < goalM_.size(); ++goalIndex)
419  {
420  if (startComponent == (long int)vertexComponentProperty_[goalM_[goalIndex]])
421  {
422  startGoalPair->first = startIndex;
423  startGoalPair->second = goalIndex;
424  return startComponent;
425  }
426  }
427  }
428  return -1;
429 }
430 
432 {
433  // Need to update the index map here, becuse nodes may have been removed and
434  // the numbering will not be 0 .. N-1 otherwise.
435  unsigned long int index = 0;
436  boost::graph_traits<Graph>::vertex_iterator vi, vend;
437  for (boost::tie(vi, vend) = boost::vertices(g_); vi != vend; ++vi, ++index)
438  indexProperty_[*vi] = index;
439 
440  boost::property_map<Graph, boost::vertex_predecessor_t>::type prev;
441  try
442  {
443  // Consider using a persistent distance_map if it's slow
444  boost::astar_search(g_, start,
445  [this, goal](Vertex v)
446  {
447  return costHeuristic(v, goal);
448  },
449  boost::predecessor_map(prev)
450  .distance_compare([this](base::Cost c1, base::Cost c2)
451  {
452  return opt_->isCostBetterThan(c1, c2);
453  })
454  .distance_combine([this](base::Cost c1, base::Cost c2)
455  {
456  return opt_->combineCosts(c1, c2);
457  })
458  .distance_inf(opt_->infiniteCost())
459  .distance_zero(opt_->identityCost())
460  .visitor(AStarGoalVisitor<Vertex>(goal)));
461  }
462  catch (AStarFoundGoal &)
463  {
464  }
465  if (prev[goal] == goal)
466  throw Exception(name_, "Could not find solution path");
467 
468  // First, get the solution states without copying them, and check them for validity.
469  // We do all the node validity checks for the vertices, as this may remove a larger
470  // part of the graph (compared to removing an edge).
471  std::vector<const base::State *> states(1, stateProperty_[goal]);
472  std::set<Vertex> milestonesToRemove;
473  for (Vertex pos = prev[goal]; prev[pos] != pos; pos = prev[pos])
474  {
475  const base::State *st = stateProperty_[pos];
476  unsigned int &vd = vertexValidityProperty_[pos];
477  if ((vd & VALIDITY_TRUE) == 0)
478  if (si_->isValid(st))
479  vd |= VALIDITY_TRUE;
480  if ((vd & VALIDITY_TRUE) == 0)
481  milestonesToRemove.insert(pos);
482  if (milestonesToRemove.empty())
483  states.push_back(st);
484  }
485 
486  // We remove *all* invalid vertices. This is not entirely as described in the original LazyPRM
487  // paper, as the paper suggest removing the first vertex only, and then recomputing the
488  // shortest path. Howeve, the paper says the focus is on efficient vertex & edge removal,
489  // rather than collision checking, so this modification is in the spirit of the paper.
490  if (!milestonesToRemove.empty())
491  {
492  unsigned long int comp = vertexComponentProperty_[start];
493  // Remember the current neighbors.
494  std::set<Vertex> neighbors;
495  for (auto it = milestonesToRemove.begin(); it != milestonesToRemove.end(); ++it)
496  {
497  boost::graph_traits<Graph>::adjacency_iterator nbh, last;
498  for (boost::tie(nbh, last) = boost::adjacent_vertices(*it, g_); nbh != last; ++nbh)
499  if (milestonesToRemove.find(*nbh) == milestonesToRemove.end())
500  neighbors.insert(*nbh);
501  // Remove vertex from nearest neighbors data structure.
502  nn_->remove(*it);
503  // Free vertex state.
504  si_->freeState(stateProperty_[*it]);
505  // Remove all edges.
506  boost::clear_vertex(*it, g_);
507  // Remove the vertex.
508  boost::remove_vertex(*it, g_);
509  }
510  // Update the connected component ID for neighbors.
511  for (auto neighbor : neighbors)
512  {
513  if (comp == vertexComponentProperty_[neighbor])
514  {
515  unsigned long int newComponent = componentCount_++;
516  componentSize_[newComponent] = 0;
517  markComponent(neighbor, newComponent);
518  }
519  }
520  return base::PathPtr();
521  }
522 
523  // start is checked for validity already
524  states.push_back(stateProperty_[start]);
525 
526  // Check the edges too, if the vertices were valid. Remove the first invalid edge only.
527  std::vector<const base::State *>::const_iterator prevState = states.begin(), state = prevState + 1;
528  Vertex prevVertex = goal, pos = prev[goal];
529  do
530  {
531  Edge e = boost::lookup_edge(pos, prevVertex, g_).first;
532  unsigned int &evd = edgeValidityProperty_[e];
533  if ((evd & VALIDITY_TRUE) == 0)
534  {
535  if (si_->checkMotion(*state, *prevState))
536  evd |= VALIDITY_TRUE;
537  }
538  if ((evd & VALIDITY_TRUE) == 0)
539  {
540  boost::remove_edge(e, g_);
541  unsigned long int newComponent = componentCount_++;
542  componentSize_[newComponent] = 0;
543  markComponent(pos, newComponent);
544  return base::PathPtr();
545  }
546  prevState = state;
547  ++state;
548  prevVertex = pos;
549  pos = prev[pos];
550  } while (prevVertex != pos);
551 
552  auto p(std::make_shared<PathGeometric>(si_));
553  for (std::vector<const base::State *>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
554  p->append(*st);
555  return p;
556 }
557 
559 {
560  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
561 }
562 
564 {
565  Planner::getPlannerData(data);
566 
567  // Explicitly add start and goal states. Tag all states known to be valid as 1.
568  // Unchecked states are tagged as 0.
569  for (auto i : startM_)
571 
572  for (auto i : goalM_)
574 
575  // Adding edges and all other vertices simultaneously
576  foreach (const Edge e, boost::edges(g_))
577  {
578  const Vertex v1 = boost::source(e, g_);
579  const Vertex v2 = boost::target(e, g_);
581 
582  // Add the reverse edge, since we're constructing an undirected roadmap
584 
585  // Add tags for the newly added vertices
586  data.tagState(stateProperty_[v1], (vertexValidityProperty_[v1] & VALIDITY_TRUE) == 0 ? 0 : 1);
587  data.tagState(stateProperty_[v2], (vertexValidityProperty_[v2] & VALIDITY_TRUE) == 0 ? 0 : 1);
588  }
589 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
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:408
void freeMemory()
Free all the memory allocated by the planner.
Definition: LazyPRM.cpp:221
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:208
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:111
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:563
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
boost::property_map< Graph, boost::vertex_index_t >::type indexProperty_
Access to the internal base::state at each Vertex.
Definition: LazyPRM.h:334
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
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:269
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:254
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...
STL namespace.
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:195
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: LazyPRM.h:322
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
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:438
Abstract definition of a goal region that can be sampled.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
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:228
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:413
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
Definition: LazyPRM.cpp:177
#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:558
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:431
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:201
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:421
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
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:168
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:424
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:216
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:171
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:415
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