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();
109  tools::SelfConfig sc(si_, getName());
110  sc.configurePlannerRange(maxDistance_);
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_)
123  connectionStrategy_ = KStarStrategy<Vertex>(
124  [this]
125  {
126  return milestoneCount();
127  },
128  nn_, si_->getStateDimension());
129  else
130  connectionStrategy_ = KBoundedStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS_LAZY, maxDistance_, nn_);
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;
166  if (!userSetConnectionStrategy_)
167  connectionStrategy_ = ConnectionStrategy();
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  }
184  if (!userSetConnectionStrategy_)
185  connectionStrategy_ = ConnectionStrategy();
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;
227  vertexValidityProperty_[m] = VALIDITY_UNKNOWN;
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;
240  edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
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_)
562  data.addStartVertex(base::PlannerDataVertex(stateProperty_[i], 1));
563 
564  for (auto i : goalM_)
565  data.addGoalVertex(base::PlannerDataVertex(stateProperty_[i], 1));
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_);
572  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
573 
574  // Add the reverse edge, since we're constructing an undirected roadmap
575  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
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
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.
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
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...
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
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
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
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.
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...
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
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
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
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.
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
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:207
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
A shared pointer wrapper for ompl::base::Path.
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