BFMT.h
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: Joseph Starek (Stanford) */
36 /* Co-developers: Javier V Gomez (UC3M)*/
37 /* Algorithm design: Joseph Starek (Stanford), Ed Schmerling (Stanford), Lucas Janson (Stanford) and Marco Pavone
38  * (Stanford) */
39 /* Acknowledgements for insightful comments: Ashley Clark (Stanford) */
40 
41 #ifndef OMPL_GEOMETRIC_PLANNERS_BIDIRECTIONALFMT_H
42 #define OMPL_GEOMETRIC_PLANNERS_BIDIRECTIONALFMT_H
43 
44 #include <ompl/geometric/planners/PlannerIncludes.h>
45 #include <ompl/base/goals/GoalSampleableRegion.h>
46 #include <ompl/datastructures/NearestNeighbors.h>
47 #include <ompl/datastructures/BinaryHeap.h>
48 #include <ompl/base/OptimizationObjective.h>
49 #include <map>
50 #include <utility>
51 
52 namespace ompl
53 {
54  namespace geometric
55  {
82  class BFMT : public ompl::base::Planner
83  {
84  public:
86  enum TreeType
87  {
88  FWD = 0,
89  REV = 1
90  };
91 
94  {
95  SWAP_EVERY_TIME = 0,
96  CHOOSE_SMALLEST_Z = 1
97  };
98 
101  {
102  FEASIBILITY = 0,
103  OPTIMALITY = 1
104  };
105 
106  BFMT(const base::SpaceInformationPtr &si);
107 
108  ~BFMT() override;
109 
110  void setup() override;
111 
113 
114  void clear() override;
115 
116  void getPlannerData(base::PlannerData &data) const override;
117 
123  void setNumSamples(const unsigned int numSamples)
124  {
125  numSamples_ = numSamples;
126  }
127 
129  unsigned int getNumSamples() const
130  {
131  return numSamples_;
132  }
133 
135  void setNearestK(bool nearestK)
136  {
137  nearestK_ = nearestK;
138  }
139 
141  bool getNearestK() const
142  {
143  return nearestK_;
144  }
145 
155  void setRadiusMultiplier(const double radiusMultiplier)
156  {
157  if (radiusMultiplier <= 0.0)
158  throw Exception("Radius multiplier must be greater than zero");
159  radiusMultiplier_ = radiusMultiplier;
160  }
161 
164  double getRadiusMultiplier() const
165  {
166  return radiusMultiplier_;
167  }
168 
172  void setFreeSpaceVolume(const double freeSpaceVolume)
173  {
174  if (freeSpaceVolume < 0.0)
175  throw Exception("Free space volume should be greater than zero");
176  freeSpaceVolume_ = freeSpaceVolume;
177  }
178 
181  double getFreeSpaceVolume() const
182  {
183  return freeSpaceVolume_;
184  }
185 
188  void setCacheCC(bool ccc)
189  {
190  cacheCC_ = ccc;
191  }
192 
194  bool getCacheCC() const
195  {
196  return cacheCC_;
197  }
198 
200  void setHeuristics(bool h)
201  {
202  heuristics_ = h;
203  }
204 
207  bool getHeuristics() const
208  {
209  return heuristics_;
210  }
211 
213  void setExtendedFMT(bool e)
214  {
215  extendedFMT_ = e;
216  }
217 
219  bool getExtendedFMT() const
220  {
221  return extendedFMT_;
222  }
223 
226  void setExploration(bool balanced)
227  {
228  exploration_ = SWAP_EVERY_TIME;
229  if (balanced)
230  {
231  exploration_ = CHOOSE_SMALLEST_Z;
232  }
233  }
234 
236  bool getExploration() const
237  {
238  return (exploration_ == CHOOSE_SMALLEST_Z);
239  }
240 
244  void setTermination(bool optimality)
245  {
246  termination_ = FEASIBILITY;
247  if (optimality)
248  {
249  termination_ = OPTIMALITY;
250  }
251  }
252 
254  bool getTermination() const
255  {
256  return (termination_ == OPTIMALITY);
257  }
258 
261  void setPrecomputeNN(bool p)
262  {
263  precomputeNN_ = p;
264  }
265 
267  bool setPrecomputeNN() const
268  {
269  return precomputeNN_;
270  }
271 
274  {
275  public:
283  enum SetType
284  {
285  SET_CLOSED,
286  SET_OPEN,
287  SET_UNVISITED
288  };
289 
290  BiDirMotion(TreeType *tree) : state_(nullptr), tree_(tree)
291  {
292  parent_[FWD] = nullptr;
293  parent_[REV] = nullptr;
294  cost_[FWD] = base::Cost(0.0);
295  cost_[REV] = base::Cost(0.0);
296  hcost_[FWD] = base::Cost(0.0);
297  hcost_[REV] = base::Cost(0.0);
298  currentSet_[FWD] = SET_UNVISITED;
299  currentSet_[REV] = SET_UNVISITED;
300  }
301 
303  BiDirMotion(const base::SpaceInformationPtr &si, TreeType *tree) : state_(si->allocState()), tree_(tree)
304  {
305  parent_[FWD] = nullptr;
306  parent_[REV] = nullptr;
307  cost_[FWD] = base::Cost(0.0);
308  cost_[REV] = base::Cost(0.0);
309  hcost_[FWD] = base::Cost(0.0);
310  hcost_[REV] = base::Cost(0.0);
311  currentSet_[FWD] = SET_UNVISITED;
312  currentSet_[REV] = SET_UNVISITED;
313  }
314 
315  typedef std::vector<BiDirMotion *> BiDirMotionPtrs;
316 
319 
322 
324  BiDirMotionPtrs children_[2];
325 
328 
331 
334 
337 
339  std::set<BiDirMotion *> collChecksDone_;
340 
342  inline base::Cost getCost() const
343  {
344  return this->cost_[*tree_];
345  }
346 
348  inline base::Cost getOtherCost() const
349  {
350  return this->cost_[(*tree_ + 1) % 2];
351  }
352 
354  inline void setCost(base::Cost cost)
355  {
356  this->cost_[*tree_] = cost;
357  }
358 
360  inline void setParent(BiDirMotion *parent)
361  {
362  this->parent_[*tree_] = parent;
363  }
364 
366  inline BiDirMotion *getParent() const
367  {
368  return this->parent_[*tree_];
369  }
370 
372  inline void setChildren(BiDirMotionPtrs children)
373  {
374  this->children_[*tree_] = std::move(children);
375  }
376 
378  inline BiDirMotionPtrs getChildren() const
379  {
380  return this->children_[*tree_];
381  }
382 
384  inline void setCurrentSet(SetType set)
385  {
386  this->currentSet_[*tree_] = set;
387  }
388 
390  inline SetType getCurrentSet() const
391  {
392  return this->currentSet_[*tree_];
393  }
394 
396  inline SetType getOtherSet() const
397  {
398  return this->currentSet_[(*tree_ + 1) % 2];
399  }
400 
402  inline void setTreeType(TreeType *treePtr)
403  {
404  this->tree_ = treePtr;
405  }
406 
408  inline TreeType getTreeType() const
409  {
410  return *tree_;
411  }
412 
414  void setState(base::State *state)
415  {
416  state_ = state;
417  }
418 
421  {
422  return state_;
423  }
424 
428  {
429  if (collChecksDone_.find(m) == collChecksDone_.end())
430  return false;
431  return true;
432  }
433 
436  {
437  collChecksDone_.insert(m);
438  }
439 
442  {
443  hcost_[*tree_] = h;
444  }
445 
448  {
449  return hcost_[*tree_];
450  }
451  };
452 
453  typedef std::vector<BiDirMotion *> BiDirMotionPtrs;
454 
455  protected:
458  {
459  bool operator()(const BiDirMotion *p1, const BiDirMotion *p2) const
460  {
461  if (heuristics_)
462  return (opt_->combineCosts(p1->getCost(), p1->getHeuristicCost()).value() <
463  opt_->combineCosts(p2->getCost(), p2->getHeuristicCost()).value());
464  else
465  return (p1->getCost().value() < p2->getCost().value());
466  }
467 
469  bool heuristics_;
470  };
471 
473 
475  void swapTrees();
476 
478  void useFwdTree()
479  {
480  tree_ = FWD;
481  }
482 
484  void useRevTree()
485  {
486  tree_ = REV;
487  }
488 
493  double distanceFunction(const BiDirMotion *a, const BiDirMotion *b) const
494  {
495  return opt_->motionCost(a->getState(), b->getState()).value();
496  }
497 
499  double calculateUnitBallVolume(const unsigned int dimension) const;
500 
508  double calculateRadius(unsigned int dimension, unsigned int n) const;
509 
511  void freeMemory();
512 
515  void saveNeighborhood(const std::shared_ptr<NearestNeighbors<BiDirMotion *>> &nn, BiDirMotion *m);
516 
519  void sampleFree(const std::shared_ptr<NearestNeighbors<BiDirMotion *>> &nn,
521 
524 
531  void expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point);
532 
534  bool plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&z,
536 
538  bool termination(BiDirMotion *&z, BiDirMotion *&connection_point,
540 
543 
545  void tracePath(BiDirMotion *z, BiDirMotionPtrs &path);
546 
549  void updateNeighborhood(BiDirMotion *m, const std::vector<BiDirMotion *> nbh);
550 
553 
555  unsigned int numSamples_;
556 
567 
571 
573  unsigned int collisionChecks_;
574 
576  bool nearestK_;
577 
579  double NNr_;
580 
582  unsigned int NNk_;
583 
586 
589 
592 
595 
597  std::shared_ptr<NearestNeighbors<BiDirMotion *>> nn_;
598 
601  std::map<BiDirMotion *, BiDirMotionPtrs> neighborhoods_;
602 
607  BiDirMotionBinHeap Open_[2];
608 
610  std::map<BiDirMotion *, BiDirMotionBinHeap::Element *> Open_elements[2];
611 
614 
617 
620 
623 
625  bool cacheCC_;
626 
629 
630  // For sorting a list of costs and getting only their sorted indices
632  {
633  CostIndexCompare(const std::vector<base::Cost> &costs, const base::OptimizationObjective &opt)
634  : costs_(costs), opt_(opt)
635  {
636  }
637  bool operator()(unsigned i, unsigned j)
638  {
639  return (costs_[i].value() < costs_[j].value());
640  }
641  const std::vector<base::Cost> &costs_;
643  };
644  };
645 
646  } // End "geometric" namespace
647 } // End "ompl" namespace
648 
649 #endif /* OMPL_GEOMETRIC_PLANNERS_BIDIRECTIONALFMT_H */
BiDirMotionPtrs getChildren() const
Get the children of the motion.
Definition: BFMT.h:378
double calculateUnitBallVolume(const unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:256
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner. ...
Definition: BFMT.h:181
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition: BFMT.cpp:667
base::State * heurGoalState_[2]
Goal state caching to accelerate cost to go heuristic computation.
Definition: BFMT.h:622
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:119
base::Cost hcost_[2]
The minimum cost to go of this motion (heuristically computed)
Definition: BFMT.h:336
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
Definition: BFMT.h:594
void setCurrentSet(SetType set)
Set the current set of the motion.
Definition: BFMT.h:384
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: BFMT.h:566
void setTermination(bool optimality)
Sets the termination strategy: optimality true finishes when the best possible path is found...
Definition: BFMT.h:244
void sampleFree(const std::shared_ptr< NearestNeighbors< BiDirMotion *>> &nn, const base::PlannerTerminationCondition &ptc)
Sample a state from the free configuration space and save it into the nearest neighbors data structur...
Definition: BFMT.cpp:228
base::StateSamplerPtr sampler_
State sampler.
Definition: BFMT.h:613
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition: BFMT.cpp:274
ExploreType exploration_
Exploration strategy used.
Definition: BFMT.h:588
A shared pointer wrapper for ompl::base::StateSampler.
void setPrecomputeNN(bool p)
Sets Nearest Neighbors precomputation. Currently, it precomputes once solve() has been called...
Definition: BFMT.h:261
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BFMT.cpp:61
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition: BFMT.h:219
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: BFMT.cpp:284
void setChildren(BiDirMotionPtrs children)
Set the children of the motion.
Definition: BFMT.h:372
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
Definition: BFMT.h:188
double distanceFunction(const BiDirMotion *a, const BiDirMotion *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: BFMT.h:493
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: BFMT.h:135
bool setPrecomputeNN() const
Returns true if Nearest Neighbor precomputation is done.
Definition: BFMT.h:267
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition: BFMT.cpp:776
BiDirMotionPtrs children_[2]
The set of motions descending from the current motion.
Definition: BFMT.h:324
void setParent(BiDirMotion *parent)
Set the parent of the motion.
Definition: BFMT.h:360
base::Cost cost_[2]
The cost of this motion.
Definition: BFMT.h:333
unsigned int NNk_
K used in the nearestK strategy.
Definition: BFMT.h:582
base::State * getState() const
Get the state associated with the motion.
Definition: BFMT.h:420
void setTreeType(TreeType *treePtr)
Set tree identifier for this motion.
Definition: BFMT.h:402
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: BFMT.h:213
SetType currentSet_[2]
Current set in which the motion is included.
Definition: BFMT.h:327
void freeMemory()
Free the memory allocated by this planner.
Definition: BFMT.cpp:105
bool getCacheCC() const
Get the state of the collision check caching.
Definition: BFMT.h:194
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: BFMT.h:616
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition: BFMT.cpp:843
ExploreType
Exploration strategy identifier.
Definition: BFMT.h:93
bool alreadyCC(BiDirMotion *m)
Returns true if the connection to m has been already tested and failed because of a collision...
Definition: BFMT.h:427
Abstract definition of a goal region that can be sampled.
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
Definition: BFMT.h:348
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setState(base::State *state)
Set the state associated with the motion.
Definition: BFMT.h:414
BiDirMotionBinHeap Open_[2]
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
Definition: BFMT.h:607
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: BFMT.h:164
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
Definition: BFMT.h:435
bool plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&z, const base::PlannerTerminationCondition &ptc)
Executes the actual planning algorithm, swapping and expanding the trees.
Definition: BFMT.cpp:583
void useFwdTree()
Sets forward tree active.
Definition: BFMT.h:478
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: BFMT.h:129
SetType getOtherSet() const
Get set of this motion in the inactive tree.
Definition: BFMT.h:396
Base class for a planner.
Definition: Planner.h:232
Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed by J. Starek...
Definition: BFMT.h:82
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"...
Definition: BFMT.h:283
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: BFMT.h:172
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: BFMT.h:597
BiDirMotion * parent_[2]
The parent motion in the exploration tree.
Definition: BFMT.h:321
bool getExploration() const
Returns the exploration strategy.
Definition: BFMT.h:236
bool getTermination() const
Returns the termination strategy.
Definition: BFMT.h:254
void swapTrees()
Change the active tree.
Definition: BFMT.cpp:854
TreeType getTreeType() const
Get tree identifier for this motion.
Definition: BFMT.h:408
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
base::Cost getHeuristicCost() const
Get the cost to go heuristic cost.
Definition: BFMT.h:447
A shared pointer wrapper for ompl::base::SpaceInformation.
void expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point)
Complete one iteration of the main loop of the BFMT* algorithm: Find K nearest nodes in set Unvisited...
Definition: BFMT.cpp:462
void setRadiusMultiplier(const double radiusMultiplier)
The planner searches for neighbors of a node within a cost r, where r is the value described for BFMT...
Definition: BFMT.h:155
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: BFMT.cpp:265
void useRevTree()
Sets reverse tree active.
Definition: BFMT.h:484
TreeType tree_
Active tree.
Definition: BFMT.h:585
bool cacheCC_
Flag to activate the collision check caching.
Definition: BFMT.h:625
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
Definition: BFMT.h:610
Definition of an abstract state.
Definition: State.h:49
TerminateType termination_
Termination strategy used.
Definition: BFMT.h:591
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: BFMT.h:573
base::State * state_
The state contained by the motion.
Definition: BFMT.h:318
Representation of a bidirectional motion.
Definition: BFMT.h:273
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: BFMT.h:628
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
Definition: BFMT.h:441
Abstract representation of a container that can perform nearest neighbors queries.
Abstract definition of optimization objectives.
TreeType
Tree identifier.
Definition: BFMT.h:86
std::map< BiDirMotion *, BiDirMotionPtrs > neighborhoods_
A map linking a motion to all of the motions within a distance r of that motion.
Definition: BFMT.h:601
The exception type for ompl.
Definition: Exception.h:46
void saveNeighborhood(const std::shared_ptr< NearestNeighbors< BiDirMotion *>> &nn, BiDirMotion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: BFMT.cpp:203
A shared pointer wrapper for ompl::base::OptimizationObjective.
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: BFMT.cpp:134
void updateNeighborhood(BiDirMotion *m, const std::vector< BiDirMotion *> nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
Definition: BFMT.cpp:859
unsigned int numSamples_
The number of samples to use when planning.
Definition: BFMT.h:555
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition: BFMT.cpp:800
BiDirMotion(const base::SpaceInformationPtr &si, TreeType *tree)
Constructor that allocates memory for the state.
Definition: BFMT.h:303
void setCost(base::Cost cost)
Set the cost of the motion.
Definition: BFMT.h:354
std::set< BiDirMotion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition: BFMT.h:339
double NNr_
Radius employed in the nearestR strategy.
Definition: BFMT.h:579
TerminateType
Termination strategy identifier.
Definition: BFMT.h:100
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence...
Definition: BFMT.h:570
double value() const
The value of the cost.
Definition: Cost.h:56
base::Cost getCost() const
Set the state associated with the motion.
Definition: BFMT.h:342
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: BFMT.h:207
void setExploration(bool balanced)
Sets exploration strategy: balanced true expands one tree every iteration. False will select the tree...
Definition: BFMT.h:226
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: BFMT.h:200
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: BFMT.h:576
bool getNearestK() const
Get the state of the nearestK strategy.
Definition: BFMT.h:141
bool heuristics_
Flag to activate the cost to go heuristics.
Definition: BFMT.h:619
BiDirMotion * getParent() const
Get the parent of the motion.
Definition: BFMT.h:366
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
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: BFMT.h:123
TreeType * tree_
Tree identifier.
Definition: BFMT.h:330
Comparator used to order motions in a binary heap.
Definition: BFMT.h:457
SetType getCurrentSet() const
Fet the current set of the motion.
Definition: BFMT.h:390