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 {
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
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
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
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
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
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 */
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.
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...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
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
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
Comparator used to order motions in a binary heap.
Definition: BFMT.h:456