FMT.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Autonomous Systems Laboratory, Stanford University
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 Stanford University 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 /* Authors: Ashley Clark (Stanford) and Wolfgang Pointner (AIT) */
36 /* Co-developers: Brice Rebsamen (Stanford) and Tim Wheeler (Stanford) */
37 /* Algorithm design: Lucas Janson (Stanford) and Marco Pavone (Stanford) */
38 /* Acknowledgements for insightful comments: Edward Schmerling (Stanford), Oren Salzman (Tel Aviv University), and Joseph Starek (Stanford) */
39 
40 #include <limits>
41 #include <iostream>
42 
43 #include <boost/math/constants/constants.hpp>
44 
45 #include <ompl/datastructures/BinaryHeap.h>
46 #include <ompl/tools/config/SelfConfig.h>
47 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
48 #include <ompl/geometric/planners/fmt/FMT.h>
49 
50 
51 ompl::geometric::FMT::FMT(const base::SpaceInformationPtr &si)
52  : base::Planner(si, "FMT")
53  , numSamples_(1000)
54  , radiusMultiplier_(1.1)
55 {
56  freeSpaceVolume_ = std::pow(si_->getMaximumExtent() / std::sqrt(si_->getStateDimension()), (int)si_->getStateDimension());
57  lastGoalMotion_ = NULL;
58 
60  specs_.directed = false;
61 
62  ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &FMT::setNumSamples, &FMT::getNumSamples, "10:10:10000");
63  ompl::base::Planner::declareParam<double>("radius_multiplier", this, &FMT::setRadiusMultiplier, &FMT::getRadiusMultiplier, "0.9:0.05:5.");
64  ompl::base::Planner::declareParam<double>("free_space_volume", this, &FMT::setFreeSpaceVolume, &FMT::getFreeSpaceVolume, "1.:10:1000000.");
65 }
66 
67 ompl::geometric::FMT::~FMT()
68 {
69  freeMemory();
70 }
71 
73 {
74  Planner::setup();
75 
76  /* Setup the optimization objective. If no optimization objective was
77  specified, then default to optimizing path length as computed by the
78  distance() function in the state space */
79  if (pdef_->hasOptimizationObjective())
80  opt_ = pdef_->getOptimizationObjective();
81  else
82  {
83  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", getName().c_str());
85  }
86  H_.getComparisonOperator().opt_ = opt_.get();
87 
88  if (!nn_)
89  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
90  nn_->setDistanceFunction(boost::bind(&FMT::distanceFunction, this, _1, _2));
91 }
92 
94 {
95  if (nn_)
96  {
97  std::vector<Motion*> motions;
98  motions.reserve(nn_->size());
99  nn_->list(motions);
100  for (unsigned int i = 0 ; i < motions.size() ; ++i)
101  {
102  si_->freeState(motions[i]->getState());
103  delete motions[i];
104  }
105  }
106 }
107 
109 {
110  Planner::clear();
111  lastGoalMotion_ = NULL;
112  sampler_.reset();
113  freeMemory();
114  if (nn_)
115  nn_->clear();
116  H_.clear();
117  hElements_.clear();
118  neighborhoods_.clear();
119 }
120 
122 {
123  Planner::getPlannerData(data);
124  std::vector<Motion*> motions;
125  nn_->list(motions);
126 
127  if (lastGoalMotion_)
129 
130  unsigned int size = motions.size();
131  for (unsigned int i = 0; i < size; ++i)
132  {
133  if (motions[i]->getParent() == NULL)
134  data.addStartVertex(base::PlannerDataVertex(motions[i]->getState()));
135  else
136  data.addEdge(base::PlannerDataVertex(motions[i]->getParent()->getState()),
137  base::PlannerDataVertex(motions[i]->getState()));
138  }
139 }
140 
142 {
143  // Check to see if neighborhood has not been saved yet
144  if (neighborhoods_.find(m) == neighborhoods_.end())
145  {
146  std::vector<Motion*> nbh;
147  nn_->nearestR(m, r, nbh);
148  if (!nbh.empty())
149  {
150  // Save the neighborhood but skip the first element, since it will be motion m
151  neighborhoods_[m] = std::vector<Motion*>(nbh.size() - 1, 0);
152  std::copy(nbh.begin() + 1, nbh.end(), neighborhoods_[m].begin());
153  }
154  else
155  {
156  // Save an empty neighborhood
157  neighborhoods_[m] = std::vector<Motion*>(0);
158  }
159  } // If neighborhood hadn't been saved yet
160 }
161 
162 // Calculate the unit ball volume for a given dimension
163 double ompl::geometric::FMT::calculateUnitBallVolume(const unsigned int dimension) const
164 {
165  if (dimension == 0)
166  return 1.0;
167  else if (dimension == 1)
168  return 2.0;
169  return 2.0 * boost::math::constants::pi<double>() / dimension
170  * calculateUnitBallVolume(dimension - 2);
171 }
172 
173 double ompl::geometric::FMT::calculateRadius(const unsigned int dimension, const unsigned int n) const
174 {
175  double a = 1.0 / (double)dimension;
176  double unitBallVolume = calculateUnitBallVolume(dimension);
177 
178  return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) * std::pow(log((double)n) / (double)n, a);
179 }
180 
182 {
183  unsigned int nodeCount = 0;
184  Motion *motion = new Motion(si_);
185 
186  // Sample numSamples_ number of nodes from the free configuration space
187  while (nodeCount < numSamples_ && !ptc)
188  {
189  sampler_->sampleUniform(motion->getState());
190 
191  bool collision_free = si_->isValid(motion->getState());
192 
193  if (collision_free)
194  {
195  nodeCount++;
196  nn_->add(motion);
197  motion = new Motion(si_);
198  } // If collision free
199  } // While nodeCount < numSamples
200  si_->freeState(motion->getState());
201  delete motion;
202 }
203 
205 {
206  // Ensure that there is at least one node near each goal
207  while (const base::State *goalState = pis_.nextGoal())
208  {
209  Motion *gMotion = new Motion(si_);
210  si_->copyState(gMotion->getState(), goalState);
211 
212  std::vector<Motion*> nearGoal;
213  nn_->nearestR(gMotion, goal->getThreshold(), nearGoal);
214 
215  // If there is no node in the goal region, insert one
216  if (nearGoal.empty())
217  {
218  OMPL_DEBUG("No state inside goal region");
219  if (si_->getStateValidityChecker()->isValid(gMotion->getState()))
220  {
221  gMotion->setSetType(Motion::SET_W);
222  nn_->add(gMotion);
223  }
224  else
225  {
226  si_->freeState(gMotion->getState());
227  delete gMotion;
228  }
229  }
230  else // There is already a sample in the goal region
231  {
232  si_->freeState(gMotion->getState());
233  delete gMotion;
234  }
235  } // For each goal
236 }
237 
239 {
240  if (lastGoalMotion_) {
241  OMPL_INFORM("solve() called before clear(); returning previous solution");
243  OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
244  return base::PlannerStatus(true, false);
245  }
246  else if (hElements_.size() > 0)
247  {
248  OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
249  clear();
250  }
251 
252  checkValidity();
253  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
254  Motion *initMotion = NULL;
255 
256  if (!goal)
257  {
258  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
260  }
261 
262  // Add start states to V (nn_) and H
263  while (const base::State *st = pis_.nextStart())
264  {
265  initMotion = new Motion(si_);
266  si_->copyState(initMotion->getState(), st);
267  hElements_[initMotion] = H_.insert(initMotion);
268  initMotion->setSetType(Motion::SET_H);
269  initMotion->setCost(opt_->initialCost(initMotion->getState()));
270  nn_->add(initMotion); // V <-- {x_init}
271  }
272 
273  if (!initMotion)
274  {
275  OMPL_ERROR("Start state undefined");
277  }
278 
279  // Sample N free states in the configuration space
280  if (!sampler_)
281  sampler_ = si_->allocStateSampler();
282  sampleFree(ptc);
283  assureGoalIsSampled(goal);
284  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
285 
286  // Calculate the nearest neighbor search radius
287  double r = calculateRadius(si_->getStateDimension(), nn_->size());
288  OMPL_DEBUG("Using radius of %f", r);
289 
290  // Flag all nodes as in set W
291  std::vector<Motion*> vNodes;
292  vNodes.reserve(nn_->size());
293  nn_->list(vNodes);
294  unsigned int vNodesSize = vNodes.size();
295  for (unsigned int i = 0; i < vNodesSize; ++i)
296  {
297  vNodes[i]->setSetType(Motion::SET_W);
298  }
299 
300  // Execute the planner, and return early if the planner returns a failure
301  bool plannerSuccess = false;
302  bool successfulExpansion = false;
303  Motion *z = initMotion; // z <-- xinit
304  z->setSetType(Motion::SET_H);
305  saveNeighborhood(z, r);
306 
307  while (!ptc && !(plannerSuccess = goal->isSatisfied(z->getState())))
308  {
309  successfulExpansion = expandTreeFromNode(z, r);
310  if (!successfulExpansion)
311  return base::PlannerStatus(false, false);
312  } // While not at goal
313 
314  if (plannerSuccess)
315  {
316  // Return the path to z, since by definition of planner success, z is in the goal region
317  lastGoalMotion_ = z;
319 
320  OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
321  return base::PlannerStatus(true, false);
322  } // if plannerSuccess
323  else
324  {
325  // Planner terminated without accomplishing goal
326  return base::PlannerStatus(false, false);
327  }
328 }
329 
331 {
332  std::vector<Motion*> mpath;
333  Motion *solution = goalMotion;
334 
335  // Construct the solution path
336  while (solution != NULL)
337  {
338  mpath.push_back(solution);
339  solution = solution->getParent();
340  }
341 
342  // Set the solution path
343  PathGeometric *path = new PathGeometric(si_);
344  int mPathSize = mpath.size();
345  for (int i = mPathSize - 1 ; i >= 0 ; --i)
346  path->append(mpath[i]->getState());
347  pdef_->addSolutionPath(base::PathPtr(path), false, -1.0, getName());
348 }
349 
351 {
352  // Find all nodes that are near z, and also in set W
353 
354  std::vector<Motion*> xNear;
355  const std::vector<Motion*> &zNeighborhood = neighborhoods_[z];
356  unsigned int zNeighborhoodSize = zNeighborhood.size();
357  xNear.reserve(zNeighborhoodSize);
358 
359  for (unsigned int i = 0; i < zNeighborhoodSize; ++i)
360  {
361  if (zNeighborhood[i]->getSetType() == Motion::SET_W)
362  xNear.push_back(zNeighborhood[i]);
363  }
364 
365  // For each node near z and in set W, attempt to connect it to set H
366  std::vector<Motion*> yNear;
367  std::vector<Motion*> H_new;
368  unsigned int xNearSize = xNear.size();
369  for (unsigned int i = 0 ; i < xNearSize; ++i)
370  {
371  Motion *x = xNear[i];
372 
373  // Find all nodes that are near x and in set H
374  saveNeighborhood(x,r);
375  const std::vector<Motion*> &xNeighborhood = neighborhoods_[x];
376 
377  unsigned int xNeighborhoodSize = xNeighborhood.size();
378  yNear.reserve(xNeighborhoodSize);
379  for (unsigned int j = 0; j < xNeighborhoodSize; ++j)
380  {
381  if (xNeighborhood[j]->getSetType() == Motion::SET_H)
382  yNear.push_back(xNeighborhood[j]);
383  }
384 
385  // Find the lowest cost-to-come connection from H to x
386  Motion *yMin = NULL;
387  base::Cost cMin(std::numeric_limits<double>::infinity());
388  unsigned int yNearSize = yNear.size();
389  for (unsigned int j = 0; j < yNearSize; ++j)
390  {
391  base::State *yState = yNear[j]->getState();
392  base::Cost dist = opt_->motionCost(yState, x->getState());
393  base::Cost cNew = opt_->combineCosts(yNear[j]->getCost(), dist);
394 
395  if (opt_->isCostBetterThan(cNew, cMin))
396  {
397  yMin = yNear[j];
398  cMin = cNew;
399  }
400  }
401  yNear.clear();
402 
403  // If an optimal connection from H to x was found
404  if (yMin != NULL)
405  {
406  bool collision_free = si_->checkMotion(yMin->getState(), x->getState());
407 
408  if (collision_free)
409  {
410  // Add edge from yMin to x
411  x->setParent(yMin);
412  x->setCost(cMin);
413  // Add x to H_new
414  H_new.push_back(x);
415  // Remove x from W
416  x->setSetType(Motion::SET_NULL);
417  }
418  } // An optimal connection from H to x was found
419  } // For each node near z and in set W, try to connect it to set H
420 
421  // Remove motion z from the binary heap and from the map
422  H_.remove(hElements_[z]);
423  hElements_.erase(z);
424  z->setSetType(Motion::SET_NULL);
425 
426  // Add the nodes in H_new to H
427  unsigned int hNewSize = H_new.size();
428  for (unsigned int i = 0; i < hNewSize; i++)
429  {
430  hElements_[H_new[i]] = H_.insert(H_new[i]);
431  H_new[i]->setSetType(Motion::SET_H);
432  }
433 
434  H_new.clear();
435 
436  if (H_.empty())
437  {
438  OMPL_INFORM("H is empty before path was found --> no feasible path exists");
439  return false;
440  }
441 
442  // Take the top of H as the new z
443  z = H_.top()->data;
444 
445  return true;
446 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
double distanceFunction(const Motion *a, const Motion *b) const
Compute the distance between two motions as the cost between their contained states. Note that for computationally intensive cost functions, the cost between motions should be stored to avoid duplicate calculations.
Definition: FMT.h:265
MotionBinHeap H_
A binary heap for storing explored motions in cost-to-come sorted order. The motions in H have been e...
Definition: FMT.h:323
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
void sampleFree(const ompl::base::PlannerTerminationCondition &ptc)
Sample a state from the free configuration space and save it into the nearest neighbors data structur...
Definition: FMT.cpp:181
unsigned int numSamples_
The number of samples to use when planning.
Definition: FMT.h:334
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:104
void setParent(Motion *parent)
Set the parent motion of the current motion.
Definition: FMT.h:194
void setRadiusMultiplier(const double radiusMultiplier)
The planner searches for neighbors of a node within a cost r, where r is the value described for FMT*...
Definition: FMT.h:119
void clear()
Clear the heap.
Definition: BinaryHeap.h:106
virtual bool isSatisfied(const State *st) const
Equivalent to calling isSatisfied(const State *, double *) with a NULL second argument.
Definition: GoalRegion.cpp:46
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:72
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified, the default assumes an obstacle-free unit hypercube, freeSpaceVolume = (maximumExtent/sqrt(dimension))^(dimension)
Definition: FMT.h:136
base::StateSamplerPtr sampler_
State sampler.
Definition: FMT.h:357
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or NULL 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:271
void remove(Element *element)
Remove a specific element.
Definition: BinaryHeap.h:127
double calculateRadius(unsigned int dimension, unsigned int n) const
Calculate the radius to use for nearest neighbor searches, using the bound given in [L...
Definition: FMT.cpp:173
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::OptimizationObjectivePtr opt_
The cost objective function.
Definition: FMT.h:360
double freeSpaceVolume_
The volume of the free configuration space.
Definition: FMT.h:337
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::map< Motion *, std::vector< Motion * > > neighborhoods_
A map linking a motion to all of the motions within a distance r of that motion.
Definition: FMT.h:331
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: FMT.cpp:121
Representation of a motion.
Definition: FMT.h:153
void freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:93
base::Cost getCost() const
Get the cost-to-come for the current motion.
Definition: FMT.h:212
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
void setNumSamples(const unsigned int numSamples)
Set the number of states that the planner should sample. The planner will sample this number of state...
Definition: FMT.h:98
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: FMT.cpp:108
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
void traceSolutionPathThroughTree(Motion *goalMotion)
Trace the path from a goal state back to the start state and save the result as a solution in the Pro...
Definition: FMT.cpp:330
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: FMT.cpp:238
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:190
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
boost::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: FMT.h:354
Abstract definition of a goal region that can be sampled.
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: FMT.h:128
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition: FMT.h:218
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: FMT.h:104
base::State * getState() const
Get the state associated with the motion.
Definition: FMT.h:188
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...
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
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:100
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:206
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner. ...
Definition: FMT.h:145
double getThreshold() const
Get the distance to the goal that is allowed for a state to be considered in the goal region...
Definition: GoalRegion.h:88
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: FMT.h:363
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
For each goal region, check to see if any of the sampled states fall within that region. If not, add a goal state from that region directly into the set of vertices. In this way, FMT is able to find a solution, if one exists. If no sampled nodes are within a goal region, there would be no way for the algorithm to successfully find a path to that region.
Definition: FMT.cpp:204
Element * top() const
Return the top element. NULL for an empty heap.
Definition: BinaryHeap.h:115
double calculateUnitBallVolume(const unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:163
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
LessThan & getComparisonOperator()
Return a reference to the comparison operator.
Definition: BinaryHeap.h:230
double value() const
The value of the cost.
Definition: Cost.h:54
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: FMT.h:351
Definition of a geometric path.
Definition: PathGeometric.h:60
bool expandTreeFromNode(Motion *&z, const double r)
Complete one iteration of the main loop of the FMT* algorithm: Find all nodes in set W within a radiu...
Definition: FMT.cpp:350
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
void saveNeighborhood(Motion *m, const double r)
Save the neighbors within a given radius of a state.
Definition: FMT.cpp:141
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:135
A boost shared pointer wrapper for ompl::base::Path.
std::map< Motion *, MotionBinHeap::Element * > hElements_
A map of all of the elements stored within the MotionBinHeap H, used to convert between Motion and El...
Definition: FMT.h:327
Motion * getParent() const
Get the parent motion of the current motion.
Definition: FMT.h:200
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68