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