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 
93  enum ExploreType
94  {
95  SWAP_EVERY_TIME = 0,
96  CHOOSE_SMALLEST_Z = 1
97  };
98 
100  enum TerminateType
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 
112  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
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 
273  class BiDirMotion
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 
321  BiDirMotion *parent_[2];
322 
324  BiDirMotionPtrs children_[2];
325 
327  SetType currentSet_[2];
328 
330  TreeType *tree_;
331 
333  base::Cost cost_[2];
334 
336  base::Cost hcost_[2];
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 
427  bool alreadyCC(BiDirMotion *m)
428  {
429  return !(collChecksDone_.find(m) == collChecksDone_.end());
430  }
431 
433  void addCC(BiDirMotion *m)
434  {
435  collChecksDone_.insert(m);
436  }
437 
439  void setHeuristicCost(const base::Cost h)
440  {
441  hcost_[*tree_] = h;
442  }
443 
446  {
447  return hcost_[*tree_];
448  }
449  };
450 
451  using BiDirMotionPtrs = std::vector<BiDirMotion *>;
452 
453  protected:
455  struct BiDirMotionCompare
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 
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 
567  double freeSpaceVolume_;
568 
570  unsigned int collisionChecks_{0u};
571 
573  bool nearestK_{true};
574 
576  double NNr_{0.};
577 
579  unsigned int NNk_{0};
580 
582  TreeType tree_{FWD};
583 
585  ExploreType exploration_{SWAP_EVERY_TIME};
586 
588  TerminateType termination_{OPTIMALITY};
589 
591  bool precomputeNN_{false};
592 
594  std::shared_ptr<NearestNeighbors<BiDirMotion *>> nn_;
595 
598  std::map<BiDirMotion *, BiDirMotionPtrs> neighborhoods_;
599 
604  BiDirMotionBinHeap Open_[2];
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 
619  base::State *heurGoalState_[2];
620 
622  bool cacheCC_{true};
623 
625  bool extendedFMT_{true};
626 
627  // For sorting a list of costs and getting only their sorted indices
628  struct CostIndexCompare
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:279
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:712
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition: BFMT.h:315
base::Cost getHeuristicCost() const
Get the cost to go heuristic cost.
Definition: BFMT.h:541
base::Cost cost_[2]
The cost of this motion
Definition: BFMT.h:429
TerminateType
Termination strategy identifier.
Definition: BFMT.h:196
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"....
Definition: BFMT.h:379
BiDirMotionPtrs getChildren() const
Get the children of the motion.
Definition: BFMT.h:474
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:715
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: BFMT.h:260
Definition of an abstract state.
Definition: State.h:113
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
Definition: BFMT.h:535
base::State * state_
The state contained by the motion.
Definition: BFMT.h:414
bool getNearestK() const
Get the state of the nearestK strategy.
Definition: BFMT.h:237
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: BFMT.h:309
double NNr_
Radius employed in the nearestR strategy.
Definition: BFMT.h:672
unsigned int NNk_
K used in the nearestK strategy.
Definition: BFMT.h:675
Comparator used to order motions in a binary heap.
Definition: BFMT.h:551
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: BFMT.h:709
double value() const
The value of the cost.
Definition: Cost.h:152
std::set< BiDirMotion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition: BFMT.h:435
bool cacheCC_
Flag to activate the collision check caching.
Definition: BFMT.h:718
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: BFMT.h:303
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
Definition: BFMT.h:529
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: BFMT.h:666
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:690
bool setPrecomputeNN() const
Returns true if Nearest Neighbor precomputation is done.
Definition: BFMT.h:363
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: BFMT.h:659
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: BFMT.h:225
Abstract definition of optimization objectives.
base::StateSamplerPtr sampler_
State sampler.
Definition: BFMT.h:706
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:251
bool getCacheCC() const
Get the state of the collision check caching.
Definition: BFMT.h:290
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence.
Definition: BFMT.h:663
BiDirMotionPtrs children_[2]
The set of motions descending from the current motion
Definition: BFMT.h:420
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
Definition: BFMT.h:703
TerminateType termination_
Termination strategy used.
Definition: BFMT.h:684
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:586
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
Definition: BFMT.h:687
TreeType
Tree identifier.
Definition: BFMT.h:182
ExploreType
Exploration strategy identifier.
Definition: BFMT.h:189
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: BFMT.h:231
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: BFMT.h:296
base::Cost getCost() const
Set the state associated with the motion.
Definition: BFMT.h:438
Abstract representation of a container that can perform nearest neighbors queries.
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: BFMT.h:669
unsigned int numSamples_
The number of samples to use when planning.
Definition: BFMT.h:648
void setTermination(bool optimality)
Sets the termination strategy: optimality true finishes when the best possible path is found....
Definition: BFMT.h:340
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified,...
Definition: BFMT.h:268
void setCost(base::Cost cost)
Set the cost of the motion.
Definition: BFMT.h:450
bool getExploration() const
Returns the exploration strategy.
Definition: BFMT.h:332
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:510
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:350
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:681
void setCurrentSet(SetType set)
Set the current set of the motion.
Definition: BFMT.h:480
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:456
BiDirMotion * getParent() const
Get the parent of the motion.
Definition: BFMT.h:462
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:426
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: BFMT.h:721
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:423
TreeType tree_
Active tree.
Definition: BFMT.h:678
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:523
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:700
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:571
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:498
void setChildren(BiDirMotionPtrs children)
Set the children of the motion.
Definition: BFMT.h:468
base::State * getState() const
Get the state associated with the motion.
Definition: BFMT.h:516
SetType getCurrentSet() const
Fet the current set of the motion.
Definition: BFMT.h:486
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:417
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:219
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:284
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:432
void useRevTree()
Sets reverse tree active.
Definition: BFMT.h:577
SetType getOtherSet() const
Get set of this motion in the inactive tree.
Definition: BFMT.h:492
TreeType getTreeType() const
Get tree identifier for this motion.
Definition: BFMT.h:504
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
Definition: BFMT.h:444
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner.
Definition: BFMT.h:277
void setExploration(bool balanced)
Sets exploration strategy: balanced true expands one tree every iteration. False will select the tree...
Definition: BFMT.h:322
Main namespace. Contains everything in this library.
Representation of a bidirectional motion.
Definition: BFMT.h:369
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:694