Loading...
Searching...
No Matches
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
52namespace ompl
53{
54 namespace geometric
55 {
82 class BFMT : public ompl::base::Planner
83 {
84 public:
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
273 class BiDirMotion
274 {
275 public:
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
328
331
334
337
339 std::set<BiDirMotion *> collChecksDone_;
340
342 inline base::Cost getCost() const
343 {
344 return this->cost_[*tree_];
345 }
346
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
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
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
476 {
477 tree_ = FWD;
478 }
479
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
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
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
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 */
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition BinaryHeap.h:53
The exception type for ompl.
Definition Exception.h:47
Abstract representation of a container that can perform nearest neighbors queries.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
Abstract definition of a goal region that can be sampled.
Abstract definition of optimization objectives.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
Definition of an abstract state.
Definition State.h:50
Representation of a bidirectional motion.
Definition BFMT.h:274
SetType getCurrentSet() const
Fet the current set of the motion.
Definition BFMT.h:390
BiDirMotion * getParent() const
Get the parent of the motion.
Definition BFMT.h:366
base::Cost getHeuristicCost() const
Get the cost to go heuristic cost.
Definition BFMT.h:445
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
TreeType * tree_
Tree identifier.
Definition BFMT.h:330
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
Definition BFMT.h:439
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"....
Definition BFMT.h:284
void setCost(base::Cost cost)
Set the cost of the motion.
Definition BFMT.h:354
BiDirMotion * parent_[2]
The parent motion in the exploration tree.
Definition BFMT.h:321
void setChildren(BiDirMotionPtrs children)
Set the children of the motion.
Definition BFMT.h:372
base::Cost cost_[2]
The cost of this motion.
Definition BFMT.h:333
base::State * state_
The state contained by the motion.
Definition BFMT.h:318
BiDirMotionPtrs children_[2]
The set of motions descending from the current motion.
Definition BFMT.h:324
BiDirMotion(const base::SpaceInformationPtr &si, TreeType *tree)
Constructor that allocates memory for the state.
Definition BFMT.h:303
SetType getOtherSet() const
Get set of this motion in the inactive tree.
Definition BFMT.h:396
base::State * getState() const
Get the state associated with the motion.
Definition BFMT.h:420
BiDirMotionPtrs getChildren() const
Get the children of the motion.
Definition BFMT.h:378
void setState(base::State *state)
Set the state associated with the motion.
Definition BFMT.h:414
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
Definition BFMT.h:433
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
Definition BFMT.h:348
SetType currentSet_[2]
Current set in which the motion is included.
Definition BFMT.h:327
void setParent(BiDirMotion *parent)
Set the parent of the motion.
Definition BFMT.h:360
void setCurrentSet(SetType set)
Set the current set of the motion.
Definition BFMT.h:384
base::Cost hcost_[2]
The minimum cost to go of this motion (heuristically computed)
Definition BFMT.h:336
base::Cost getCost() const
Set the state associated with the motion.
Definition BFMT.h:342
std::set< BiDirMotion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition BFMT.h:339
void setTreeType(TreeType *treePtr)
Set tree identifier for this motion.
Definition BFMT.h:402
TreeType getTreeType() const
Get tree identifier for this motion.
Definition BFMT.h:408
Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed by J. Starek,...
Definition BFMT.h:83
TerminateType termination_
Termination strategy used.
Definition BFMT.h:588
double NNr_
Radius employed in the nearestR strategy.
Definition BFMT.h:576
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition BFMT.h:563
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
Definition BFMT.h:607
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 insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition BFMT.cpp:655
void setExploration(bool balanced)
Sets exploration strategy: balanced true expands one tree every iteration. False will select the tree...
Definition BFMT.h:226
base::State * heurGoalState_[2]
Goal state caching to accelerate cost to go heuristic computation.
Definition BFMT.h:619
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
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition BFMT.cpp:830
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner.
Definition BFMT.h:181
TreeType tree_
Active tree.
Definition BFMT.h:582
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition BFMT.cpp:763
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition BFMT.h:570
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
bool getCacheCC() const
Get the state of the collision check caching.
Definition BFMT.h:194
void freeMemory()
Free the memory allocated by this planner.
Definition BFMT.cpp:92
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
void setTermination(bool optimality)
Sets the termination strategy: optimality true finishes when the best possible path is found....
Definition BFMT.h:244
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
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition BFMT.h:594
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
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
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition BFMT.h:207
ExploreType exploration_
Exploration strategy used.
Definition BFMT.h:585
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition BFMT.cpp:261
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
TerminateType
Termination strategy identifier.
Definition BFMT.h:101
bool heuristics_
Flag to activate the cost to go heuristics.
Definition BFMT.h:616
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
base::StateSamplerPtr sampler_
State sampler.
Definition BFMT.h:610
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
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition BFMT.h:613
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition BFMT.cpp:787
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
ExploreType
Exploration strategy identifier.
Definition BFMT.h:94
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition BFMT.h:219
unsigned int NNk_
K used in the nearestK strategy.
Definition BFMT.h:579
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
void useFwdTree()
Sets forward tree active.
Definition BFMT.h:475
bool getTermination() const
Returns the termination strategy.
Definition BFMT.h:254
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
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
unsigned int numSamples_
The number of samples to use when planning.
Definition BFMT.h:552
void useRevTree()
Sets reverse tree active.
Definition BFMT.h:481
void setPrecomputeNN(bool p)
Sets Nearest Neighbors precomputation. Currently, it precomputes once solve() has been called.
Definition BFMT.h:261
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence.
Definition BFMT.h:567
void swapTrees()
Change the active tree.
Definition BFMT.cpp:841
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition BFMT.h:213
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition BFMT.h:129
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
Definition BFMT.h:591
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified,...
Definition BFMT.h:172
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition BFMT.cpp:106
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition BFMT.h:200
TreeType
Tree identifier.
Definition BFMT.h:87
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition BFMT.h:164
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 cacheCC_
Flag to activate the collision check caching.
Definition BFMT.h:622
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition BFMT.h:135
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition BFMT.h:573
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition BFMT.h:625
bool getNearestK() const
Get the state of the nearestK strategy.
Definition BFMT.h:141
bool setPrecomputeNN() const
Returns true if Nearest Neighbor precomputation is done.
Definition BFMT.h:267
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Comparator used to order motions in a binary heap.
Definition BFMT.h:456