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
260 Motion *getParent() const
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
291 bool alreadyCC(Motion *m)
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
344 struct MotionCompare
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
492 struct CostIndexCompare
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_
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
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.
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
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
ompl::BinaryHeap< Motion *, MotionCompare > MotionBinHeap
A binary heap for storing explored motions in cost-to-come sorted order.
Definition FMT.h:424
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
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()