Loading...
Searching...
No Matches
FMT.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: Ashley Clark (Stanford) and Wolfgang Pointner (AIT) */
36/* Co-developers: Brice Rebsamen (Stanford), Tim Wheeler (Stanford)
37 Edward Schmerling (Stanford), and Javier V. Gómez (UC3M - Stanford)*/
38/* Algorithm design: Lucas Janson (Stanford) and Marco Pavone (Stanford) */
39/* Acknowledgements for insightful comments: Oren Salzman (Tel Aviv University),
40 * Joseph Starek (Stanford) */
41
42#ifndef OMPL_GEOMETRIC_PLANNERS_FMT_
43#define OMPL_GEOMETRIC_PLANNERS_FMT_
44
45#include <ompl/geometric/planners/PlannerIncludes.h>
46#include <ompl/base/goals/GoalSampleableRegion.h>
47#include <ompl/datastructures/NearestNeighbors.h>
48#include <ompl/datastructures/BinaryHeap.h>
49#include <ompl/base/OptimizationObjective.h>
50#include <map>
51
52namespace ompl
53{
54 namespace geometric
55 {
90 class FMT : public ompl::base::Planner
91 {
92 public:
93 FMT(const base::SpaceInformationPtr &si);
94
95 ~FMT() override;
96
97 void setup() override;
98
100
101 void clear() override;
102
103 void getPlannerData(base::PlannerData &data) const override;
104
110 void setNumSamples(const unsigned int numSamples)
111 {
112 numSamples_ = numSamples;
113 }
114
116 unsigned int getNumSamples() const
117 {
118 return numSamples_;
119 }
120
122 void setNearestK(bool nearestK)
123 {
124 nearestK_ = nearestK;
125 }
126
128 bool getNearestK() const
129 {
130 return nearestK_;
131 }
132
142 void setRadiusMultiplier(const double radiusMultiplier)
143 {
144 if (radiusMultiplier <= 0.0)
145 throw Exception("Radius multiplier must be greater than zero");
146 radiusMultiplier_ = radiusMultiplier;
147 }
148
151 double getRadiusMultiplier() const
152 {
153 return radiusMultiplier_;
154 }
155
159 void setFreeSpaceVolume(const double freeSpaceVolume)
160 {
161 if (freeSpaceVolume < 0.0)
162 throw Exception("Free space volume should be greater than zero");
163 freeSpaceVolume_ = freeSpaceVolume;
164 }
165
168 double getFreeSpaceVolume() const
169 {
170 return freeSpaceVolume_;
171 }
172
175 void setCacheCC(bool ccc)
176 {
177 cacheCC_ = ccc;
178 }
179
181 bool getCacheCC() const
182 {
183 return cacheCC_;
184 }
185
187 void setHeuristics(bool h)
188 {
189 heuristics_ = h;
190 }
191
194 bool getHeuristics() const
195 {
196 return heuristics_;
197 }
198
200 void setExtendedFMT(bool e)
201 {
202 extendedFMT_ = e;
203 }
204
206 bool getExtendedFMT() const
207 {
208 return extendedFMT_;
209 }
210
211 protected:
214 class Motion
215 {
216 public:
225 {
226 SET_CLOSED,
227 SET_OPEN,
228 SET_UNVISITED
229 };
230
231 Motion() = default;
232
234 Motion(const base::SpaceInformationPtr &si)
235 : state_(si->allocState())
236 {
237 }
238
239 ~Motion() = default;
240
243 {
244 state_ = state;
245 }
246
249 {
250 return state_;
251 }
252
254 void setParent(Motion *parent)
255 {
256 parent_ = parent;
257 }
258
261 {
262 return parent_;
263 }
264
266 void setCost(const base::Cost cost)
267 {
268 cost_ = cost;
269 }
270
273 {
274 return cost_;
275 }
276
278 void setSetType(const SetType currentSet)
279 {
280 currentSet_ = currentSet;
281 }
282
285 {
286 return currentSet_;
287 }
288
292 {
293 return !(collChecksDone_.find(m) == collChecksDone_.end());
294 }
295
297 void addCC(Motion *m)
298 {
299 collChecksDone_.insert(m);
300 }
301
304 {
305 hcost_ = h;
306 }
307
310 {
311 return hcost_;
312 }
313
315 std::vector<Motion *> &getChildren()
316 {
317 return children_;
318 }
319
320 protected:
323
325 Motion *parent_{nullptr};
326
329
332
334 SetType currentSet_{SET_UNVISITED};
335
337 std::set<Motion *> collChecksDone_;
338
340 std::vector<Motion *> children_;
341 };
342
345 {
346 MotionCompare() = default;
347
348 /* Returns true if m1 is lower cost than m2. m1 and m2 must
349 have been instantiated with the same optimization objective */
350 bool operator()(const Motion *m1, const Motion *m2) const
351 {
352 if (heuristics_)
353 return opt_->isCostBetterThan(opt_->combineCosts(m1->getCost(), m1->getHeuristicCost()),
354 opt_->combineCosts(m2->getCost(), m2->getHeuristicCost()));
355 return opt_->isCostBetterThan(m1->getCost(), m2->getCost());
356 }
357
358 base::OptimizationObjective *opt_{nullptr};
359 bool heuristics_{false};
360 };
361
366 double distanceFunction(const Motion *a, const Motion *b) const
367 {
368 return opt_->motionCost(a->getState(), b->getState()).value();
369 }
370
372 void freeMemory();
373
377
385
387 double calculateUnitBallVolume(unsigned int dimension) const;
388
396 double calculateRadius(unsigned int dimension, unsigned int n) const;
397
400 void saveNeighborhood(Motion *m);
401
404 void traceSolutionPathThroughTree(Motion *goalMotion);
405
412 bool expandTreeFromNode(Motion **z);
413
417 void updateNeighborhood(Motion *m, std::vector<Motion *> nbh);
418
420 Motion *getBestParent(Motion *m, std::vector<Motion *> &neighbors, base::Cost &cMin);
421
425
431
434 std::map<Motion *, std::vector<Motion *>> neighborhoods_;
435
437 unsigned int numSamples_{1000u};
438
440 unsigned int collisionChecks_{0u};
441
443 bool nearestK_{true};
444
446 bool cacheCC_{true};
447
449 bool heuristics_{false};
450
452 double NNr_;
453
455 unsigned int NNk_;
456
460
471 double radiusMultiplier_{1.1};
472
474 std::shared_ptr<NearestNeighbors<Motion *>> nn_;
475
477 base::StateSamplerPtr sampler_;
478
480 base::OptimizationObjectivePtr opt_;
481
484
487
489 bool extendedFMT_{true};
490
491 // For sorting a list of costs and getting only their sorted indices
493 {
494 CostIndexCompare(const std::vector<base::Cost> &costs, const base::OptimizationObjective &opt)
495 : costs_(costs), opt_(opt)
496 {
497 }
498 bool operator()(unsigned i, unsigned j)
499 {
500 return opt_.isCostBetterThan(costs_[i], costs_[j]);
501 }
502 const std::vector<base::Cost> &costs_;
503 const base::OptimizationObjective &opt_;
504 };
505 };
506 }
507}
508
509#endif // OMPL_GEOMETRIC_PLANNERS_FMT_
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
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...
virtual bool isCostBetterThan(Cost c1, Cost c2) const
Check whether the the cost c1 is considered better than the cost c2. By default, this returns true if...
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 motion.
Definition FMT.h:215
base::Cost cost_
The cost of this motion.
Definition FMT.h:328
void setParent(Motion *parent)
Set the parent motion of the current motion.
Definition FMT.h:254
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition FMT.h:278
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition FMT.h:234
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
Definition FMT.h:303
SetType getSetType() const
Get the set that this motion belongs to.
Definition FMT.h:284
void setState(base::State *state)
Set the state associated with the motion.
Definition FMT.h:242
base::State * getState() const
Get the state associated with the motion.
Definition FMT.h:248
void addCC(Motion *m)
Caches a failed collision check to m.
Definition FMT.h:297
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition FMT.h:266
base::Cost getCost() const
Get the cost-to-come for the current motion.
Definition FMT.h:272
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"....
Definition FMT.h:225
base::Cost hcost_
The minimum cost to go of this motion (heuristically computed)
Definition FMT.h:331
std::set< Motion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition FMT.h:337
std::vector< Motion * > children_
The set of motions descending from the current motion.
Definition FMT.h:340
base::State * state_
The state contained by the motion.
Definition FMT.h:322
std::vector< Motion * > & getChildren()
Get the children of the motion.
Definition FMT.h:315
base::Cost getHeuristicCost() const
Get the cost to go heuristic cost.
Definition FMT.h:309
bool alreadyCC(Motion *m)
Returns true if the connection to m has been already tested and failed because of a collision.
Definition FMT.h:291
Motion * parent_
The parent motion in the exploration tree.
Definition FMT.h:325
SetType currentSet_
The flag indicating which set a motion belongs to.
Definition FMT.h:334
Motion * getParent() const
Get the parent motion of the current motion.
Definition FMT.h:260
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone.
Definition FMT.h:91
unsigned int NNk_
K used in the nearestK strategy.
Definition FMT.h:455
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition FMT.h:443
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner.
Definition FMT.h:168
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition FMT.h:200
void saveNeighborhood(Motion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition FMT.cpp:169
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 FMT.cpp:203
bool cacheCC_
Flag to activate the collision check caching.
Definition FMT.h:446
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition FMT.h:194
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition FMT.h:474
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition FMT.h:122
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 FMT.cpp:276
bool heuristics_
Flag to activate the cost to go heuristics.
Definition FMT.h:449
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition FMT.h:471
void traceSolutionPathThroughTree(Motion *goalMotion)
Trace the path from a goal state back to the start state and save the result as a solution in the Pro...
Definition FMT.cpp:478
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified,...
Definition FMT.h:159
unsigned int numSamples_
The number of samples to use when planning.
Definition FMT.h:437
void updateNeighborhood(Motion *m, std::vector< Motion * > nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
Definition FMT.cpp:637
Motion * getBestParent(Motion *m, std::vector< Motion * > &neighbors, base::Cost &cMin)
Returns the best parent and the connection cost in the neighborhood of a motion m.
Definition FMT.cpp:617
void sampleFree(const ompl::base::PlannerTerminationCondition &ptc)
Sample a state from the free configuration space and save it into the nearest neighbors data structur...
Definition FMT.cpp:212
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition FMT.h:116
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition FMT.h:480
base::State * goalState_
Goal state caching to accelerate cost to go heuristic computation.
Definition FMT.h:486
double NNr_
Radius employed in the nearestR strategy.
Definition FMT.h:452
void freeMemory()
Free the memory allocated by this planner.
Definition FMT.cpp:120
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition FMT.cpp:135
double freeSpaceVolume_
The volume of the free configuration space, computed as an upper bound with 95% confidence.
Definition FMT.h:459
std::map< Motion *, std::vector< Motion * > > neighborhoods_
A map linking a motion to all of the motions within a distance r of that motion.
Definition FMT.h:434
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition FMT.h:187
base::StateSamplerPtr sampler_
State sampler.
Definition FMT.h:477
void setRadiusMultiplier(const double radiusMultiplier)
The planner searches for neighbors of a node within a cost r, where r is the value described for FMT*...
Definition FMT.h:142
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition FMT.h:483
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition FMT.h:151
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition FMT.h:440
double distanceFunction(const Motion *a, const Motion *b) const
Compute the distance between two motions as the cost between their contained states....
Definition FMT.h:366
bool expandTreeFromNode(Motion **z)
Complete one iteration of the main loop of the FMT* algorithm: Find K nearest nodes in set Unvisited ...
Definition FMT.cpp:498
MotionBinHeap Open_
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
Definition FMT.h:430
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition FMT.h:206
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
Definition FMT.h:175
bool getCacheCC() const
Get the state of the collision check caching.
Definition FMT.h:181
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition FMT.h:489
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition FMT.cpp:194
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 FMT.cpp:149
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition FMT.cpp:78
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 FMT.h:110
bool getNearestK() const
Get the state of the nearestK strategy.
Definition FMT.h:128
void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
For each goal region, check to see if any of the sampled states fall within that region....
Definition FMT.cpp:241
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 FMT.h:345