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  using BiDirMotionPtrs = std::vector<BiDirMotion *>;
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  return !(collChecksDone_.find(m) == collChecksDone_.end());
430  }
431 
434  {
435  collChecksDone_.insert(m);
436  }
437 
440  {
441  hcost_[*tree_] = h;
442  }
443 
446  {
447  return hcost_[*tree_];
448  }
449  };
450 
451  using BiDirMotionPtrs = std::vector<BiDirMotion *>;
452 
453  protected:
456  {
457  bool operator()(const BiDirMotion *p1, const BiDirMotion *p2) const
458  {
459  if (heuristics_)
460  return (opt_->combineCosts(p1->getCost(), p1->getHeuristicCost()).value() <
461  opt_->combineCosts(p2->getCost(), p2->getHeuristicCost()).value());
462  return (p1->getCost().value() < p2->getCost().value());
463  }
464 
466  bool heuristics_;
467  };
468 
470 
472  void swapTrees();
473 
475  void useFwdTree()
476  {
477  tree_ = FWD;
478  }
479 
481  void useRevTree()
482  {
483  tree_ = REV;
484  }
485 
490  double distanceFunction(const BiDirMotion *a, const BiDirMotion *b) const
491  {
492  return opt_->motionCost(a->getState(), b->getState()).value();
493  }
494 
496  double calculateUnitBallVolume(unsigned int dimension) const;
497 
505  double calculateRadius(unsigned int dimension, unsigned int n) const;
506 
508  void freeMemory();
509 
512  void saveNeighborhood(BiDirMotion *m);
513 
516  void sampleFree(const std::shared_ptr<NearestNeighbors<BiDirMotion *>> &nn,
518 
521 
528  void expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point);
529 
531  bool plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&connection_point,
533 
535  bool termination(BiDirMotion *&z, BiDirMotion *&connection_point,
537 
539  void chooseTreeAndExpansionNode(BiDirMotion *&z);
540 
542  void tracePath(BiDirMotion *z, BiDirMotionPtrs &path);
543 
546  void updateNeighborhood(BiDirMotion *m, std::vector<BiDirMotion *> nbh);
547 
550 
552  unsigned int numSamples_{1000u};
553 
563  double radiusMultiplier_{1.};
564 
568 
570  unsigned int collisionChecks_{0u};
571 
573  bool nearestK_{true};
574 
576  double NNr_{0.};
577 
579  unsigned int NNk_{0};
580 
583 
585  ExploreType exploration_{SWAP_EVERY_TIME};
586 
589 
591  bool precomputeNN_{false};
592 
594  std::shared_ptr<NearestNeighbors<BiDirMotion *>> nn_;
595 
598  std::map<BiDirMotion *, BiDirMotionPtrs> neighborhoods_;
599 
605 
607  std::map<BiDirMotion *, BiDirMotionBinHeap::Element *> Open_elements[2];
608 
610  base::StateSamplerPtr sampler_;
611 
613  base::OptimizationObjectivePtr opt_;
614 
616  bool heuristics_{true};
617 
620 
622  bool cacheCC_{true};
623 
625  bool extendedFMT_{true};
626 
627  // For sorting a list of costs and getting only their sorted indices
629  {
630  CostIndexCompare(const std::vector<base::Cost> &costs, const base::OptimizationObjective &opt)
631  : costs_(costs), opt_(opt)
632  {
633  }
634  bool operator()(unsigned i, unsigned j)
635  {
636  return (costs_[i].value() < costs_[j].value());
637  }
638  const std::vector<base::Cost> &costs_;
639  const base::OptimizationObjective &opt_;
640  };
641  };
642 
643  } // End "geometric" namespace
644 } // End "ompl" namespace
645 
646 #endif /* OMPL_GEOMETRIC_PLANNERS_BIDIRECTIONALFMT_H */
Base class for a planner.
Definition: Planner.h:223
void saveNeighborhood(BiDirMotion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: BFMT.cpp:190
bool heuristics_
Flag to activate the cost to go heuristics.
Definition: BFMT.h:616
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition: BFMT.h:219
base::Cost getHeuristicCost() const
Get the cost to go heuristic cost.
Definition: BFMT.h:445
base::Cost cost_[2]
The cost of this motion
Definition: BFMT.h:333
TerminateType
Termination strategy identifier.
Definition: BFMT.h:101
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"....
Definition: BFMT.h:284
BiDirMotionPtrs getChildren() const
Get the children of the motion.
Definition: BFMT.h:378
virtual Cost combineCosts(Cost c1, Cost c2) const
Get the cost that corresponds to combining the costs c1 and c2. Default implementation defines this c...
base::State * heurGoalState_[2]
Goal state caching to accelerate cost to go heuristic computation.
Definition: BFMT.h:619
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: BFMT.h:164
Definition of an abstract state.
Definition: State.h:50
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
Definition: BFMT.h:439
base::State * state_
The state contained by the motion.
Definition: BFMT.h:318
bool getNearestK() const
Get the state of the nearestK strategy.
Definition: BFMT.h:141
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: BFMT.h:213
void setPrecomputeNN(bool p)
Sets Nearest Neighbors precomputation. Currently, it precomputes once solve() has been called.
Definition: BFMT.h:261
double NNr_
Radius employed in the nearestR strategy.
Definition: BFMT.h:576
unsigned int NNk_
K used in the nearestK strategy.
Definition: BFMT.h:579
Comparator used to order motions in a binary heap.
Definition: BFMT.h:456
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: BFMT.h:613
double value() const
The value of the cost.
Definition: Cost.h:56
std::set< BiDirMotion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition: BFMT.h:339
bool cacheCC_
Flag to activate the collision check caching.
Definition: BFMT.h:622
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: BFMT.h:207
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
Definition: BFMT.h:433
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: BFMT.h:570
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition: BFMT.cpp:655
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: BFMT.h:594
bool setPrecomputeNN() const
Returns true if Nearest Neighbor precomputation is done.
Definition: BFMT.h:267
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: BFMT.h:563
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: BFMT.h:129
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Abstract definition of optimization objectives.
base::StateSamplerPtr sampler_
State sampler.
Definition: BFMT.h:610
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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
bool getCacheCC() const
Get the state of the collision check caching.
Definition: BFMT.h:194
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence.
Definition: BFMT.h:567
BiDirMotionPtrs children_[2]
The set of motions descending from the current motion
Definition: BFMT.h:324
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
Definition: BFMT.h:607
TerminateType termination_
Termination strategy used.
Definition: BFMT.h:588
double distanceFunction(const BiDirMotion *a, const BiDirMotion *b) const
Compute the distance between two motions as the cost between their contained states....
Definition: BFMT.h:490
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
Definition: BFMT.h:591
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
TreeType
Tree identifier.
Definition: BFMT.h:87
ExploreType
Exploration strategy identifier.
Definition: BFMT.h:94
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: BFMT.h:135
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: BFMT.h:200
base::Cost getCost() const
Set the state associated with the motion.
Definition: BFMT.h:342
Abstract representation of a container that can perform nearest neighbors queries.
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: BFMT.h:573
unsigned int numSamples_
The number of samples to use when planning.
Definition: BFMT.h:552
void setTermination(bool optimality)
Sets the termination strategy: optimality true finishes when the best possible path is found....
Definition: BFMT.h:244
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified,...
Definition: BFMT.h:172
void setCost(base::Cost cost)
Set the cost of the motion.
Definition: BFMT.h:354
bool getExploration() const
Returns the exploration strategy.
Definition: BFMT.h:236
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:243
void setState(base::State *state)
Set the state associated with the motion.
Definition: BFMT.h:414
void updateNeighborhood(BiDirMotion *m, std::vector< BiDirMotion * > nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
Definition: BFMT.cpp:846
bool getTermination() const
Returns the termination strategy.
Definition: BFMT.h:254
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:121
ExploreType exploration_
Exploration strategy used.
Definition: BFMT.h:585
void setCurrentSet(SetType set)
Set the current set of the motion.
Definition: BFMT.h:384
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition: BFMT.cpp:261
void setParent(BiDirMotion *parent)
Set the parent of the motion.
Definition: BFMT.h:360
BiDirMotion * getParent() const
Get the parent of the motion.
Definition: BFMT.h:366
void swapTrees()
Change the active tree.
Definition: BFMT.cpp:841
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:447
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:252
TreeType * tree_
Tree identifier
Definition: BFMT.h:330
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: BFMT.h:625
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:271
SetType currentSet_[2]
Current set in which the motion is included.
Definition: BFMT.h:327
TreeType tree_
Active tree.
Definition: BFMT.h:582
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:215
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:106
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:604
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition: BFMT.cpp:763
void useFwdTree()
Sets forward tree active.
Definition: BFMT.h:475
bool plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Executes the actual planning algorithm, swapping and expanding the trees.
Definition: BFMT.cpp:571
void setTreeType(TreeType *treePtr)
Set tree identifier for this motion.
Definition: BFMT.h:402
Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed by J. Starek,...
Definition: BFMT.h:83
void setChildren(BiDirMotionPtrs children)
Set the children of the motion.
Definition: BFMT.h:372
base::State * getState() const
Get the state associated with the motion.
Definition: BFMT.h:420
SetType getCurrentSet() const
Fet the current set of the motion.
Definition: BFMT.h:390
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition: BFMT.cpp:787
void freeMemory()
Free the memory allocated by this planner.
Definition: BFMT.cpp:92
BiDirMotion * parent_[2]
The parent motion in the exploration tree
Definition: BFMT.h:321
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BFMT.cpp:48
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
Abstract definition of a goal region that can be sampled.
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
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition: BFMT.cpp:830
base::Cost hcost_[2]
The minimum cost to go of this motion (heuristically computed)
Definition: BFMT.h:336
The exception type for ompl.
Definition: Exception.h:47
void useRevTree()
Sets reverse tree active.
Definition: BFMT.h:481
SetType getOtherSet() const
Get set of this motion in the inactive tree.
Definition: BFMT.h:396
TreeType getTreeType() const
Get tree identifier for this motion.
Definition: BFMT.h:408
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
Definition: BFMT.h:348
BiDirMotion(const base::SpaceInformationPtr &si, TreeType *tree)
Constructor that allocates memory for the state.
Definition: BFMT.h:303
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner.
Definition: BFMT.h:181
void setExploration(bool balanced)
Sets exploration strategy: balanced true expands one tree every iteration. False will select the tree...
Definition: BFMT.h:226
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
Representation of a bidirectional motion.
Definition: BFMT.h:274
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:598