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 
52 namespace 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:
224  enum SetType
225  {
226  SET_CLOSED,
227  SET_OPEN,
228  SET_UNVISITED
229  };
230 
231  Motion() : state_(nullptr), parent_(nullptr), cost_(0.0), currentSet_(SET_UNVISITED)
232  {
233  }
234 
237  : state_(si->allocState()), parent_(nullptr), cost_(0.0), currentSet_(SET_UNVISITED)
238  {
239  }
240 
241  ~Motion() = default;
242 
244  void setState(base::State *state)
245  {
246  state_ = state;
247  }
248 
251  {
252  return state_;
253  }
254 
256  void setParent(Motion *parent)
257  {
258  parent_ = parent;
259  }
260 
262  Motion *getParent() const
263  {
264  return parent_;
265  }
266 
268  void setCost(const base::Cost cost)
269  {
270  cost_ = cost;
271  }
272 
275  {
276  return cost_;
277  }
278 
280  void setSetType(const SetType currentSet)
281  {
282  currentSet_ = currentSet;
283  }
284 
287  {
288  return currentSet_;
289  }
290 
293  bool alreadyCC(Motion *m)
294  {
295  if (collChecksDone_.find(m) == collChecksDone_.end())
296  return false;
297  return true;
298  }
299 
301  void addCC(Motion *m)
302  {
303  collChecksDone_.insert(m);
304  }
305 
308  {
309  hcost_ = h;
310  }
311 
314  {
315  return hcost_;
316  }
317 
319  std::vector<Motion *> &getChildren()
320  {
321  return children_;
322  }
323 
324  protected:
327 
330 
333 
336 
339 
341  std::set<Motion *> collChecksDone_;
342 
344  std::vector<Motion *> children_;
345  };
346 
349  {
350  MotionCompare() : opt_(nullptr), heuristics_(false)
351  {
352  }
353 
354  /* Returns true if m1 is lower cost than m2. m1 and m2 must
355  have been instantiated with the same optimization objective */
356  bool operator()(const Motion *m1, const Motion *m2) const
357  {
358  if (heuristics_)
359  return opt_->isCostBetterThan(opt_->combineCosts(m1->getCost(), m1->getHeuristicCost()),
360  opt_->combineCosts(m2->getCost(), m2->getHeuristicCost()));
361  else
362  return opt_->isCostBetterThan(m1->getCost(), m2->getCost());
363  }
364 
366  bool heuristics_;
367  };
368 
373  double distanceFunction(const Motion *a, const Motion *b) const
374  {
375  return opt_->motionCost(a->getState(), b->getState()).value();
376  }
377 
379  void freeMemory();
380 
384 
392 
394  double calculateUnitBallVolume(const unsigned int dimension) const;
395 
403  double calculateRadius(unsigned int dimension, unsigned int n) const;
404 
407  void saveNeighborhood(Motion *m);
408 
411  void traceSolutionPathThroughTree(Motion *goalMotion);
412 
419  bool expandTreeFromNode(Motion **z);
420 
424  void updateNeighborhood(Motion *m, const std::vector<Motion *> nbh);
425 
427  Motion *getBestParent(Motion *m, std::vector<Motion *> &neighbors, base::Cost &cMin);
428 
432 
437  MotionBinHeap Open_;
438 
441  std::map<Motion *, std::vector<Motion *>> neighborhoods_;
442 
444  unsigned int numSamples_;
445 
447  unsigned int collisionChecks_;
448 
450  bool nearestK_;
451 
453  bool cacheCC_;
454 
457 
459  double NNr_;
460 
462  unsigned int NNk_;
463 
467 
479 
481  std::shared_ptr<NearestNeighbors<Motion *>> nn_;
482 
485 
488 
491 
494 
497 
498  // For sorting a list of costs and getting only their sorted indices
500  {
501  CostIndexCompare(const std::vector<base::Cost> &costs, const base::OptimizationObjective &opt)
502  : costs_(costs), opt_(opt)
503  {
504  }
505  bool operator()(unsigned i, unsigned j)
506  {
507  return opt_.isCostBetterThan(costs_[i], costs_[j]);
508  }
509  const std::vector<base::Cost> &costs_;
511  };
512  };
513  }
514 }
515 
516 #endif // OMPL_GEOMETRIC_PLANNERS_FMT_
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: FMT.h:450
bool cacheCC_
Flag to activate the collision check caching.
Definition: FMT.h:453
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: FMT.h:200
double distanceFunction(const Motion *a, const Motion *b) const
Compute the distance between two motions as the cost between their contained states. Note that for computationally intensive cost functions, the cost between motions should be stored to avoid duplicate calculations.
Definition: FMT.h:373
bool getNearestK() const
Get the state of the nearestK strategy.
Definition: FMT.h:128
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition: FMT.h:206
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:219
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: FMT.cpp:142
unsigned int numSamples_
The number of samples to use when planning.
Definition: FMT.h:444
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"...
Definition: FMT.h:224
void setParent(Motion *parent)
Set the parent motion of the current motion.
Definition: FMT.h:256
SetType getSetType() const
Get the set that this motion belongs to.
Definition: FMT.h:286
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
std::vector< Motion * > & getChildren()
Get the children of the motion.
Definition: FMT.h:319
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:156
void addCC(Motion *m)
Caches a failed collision check to m.
Definition: FMT.h:301
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified, the default assumes an obstacle-free unit hypercube, freeSpaceVolume = (maximumExtent/sqrt(dimension))^(dimension)
Definition: FMT.h:159
SetType currentSet_
The flag indicating which set a motion belongs to.
Definition: FMT.h:338
base::StateSamplerPtr sampler_
State sampler.
Definition: FMT.h:484
A shared pointer wrapper for ompl::base::StateSampler.
double NNr_
Radius employed in the nearestR strategy.
Definition: FMT.h:459
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: FMT.h:496
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:210
base::Cost cost_
The cost of this motion.
Definition: FMT.h:332
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: FMT.h:487
MotionBinHeap Open_
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
Definition: FMT.h:437
double freeSpaceVolume_
The volume of the free configuration space, computed as an upper bound with 95% confidence.
Definition: FMT.h:466
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: FMT.h:481
ompl::BinaryHeap< Motion *, MotionCompare > MotionBinHeap
A binary heap for storing explored motions in cost-to-come sorted order.
Definition: FMT.h:431
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: FMT.h:447
base::Cost getHeuristicCost() const
Get the cost to go heuristic cost.
Definition: FMT.h:313
base::Cost hcost_
The minimum cost to go of this motion (heuristically computed)
Definition: FMT.h:335
Representation of a motion.
Definition: FMT.h:214
void freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:127
base::Cost getCost() const
Get the cost-to-come for the current motion.
Definition: FMT.h:274
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 expandTreeFromNode(Motion **z)
Complete one iteration of the main loop of the FMT* algorithm: Find K nearest nodes in set Unvisited ...
Definition: FMT.cpp:508
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:488
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: FMT.h:236
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone...
Definition: FMT.h:90
Abstract definition of a goal region that can be sampled.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
unsigned int NNk_
K used in the nearestK strategy.
Definition: FMT.h:462
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:628
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: FMT.h:151
std::set< Motion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition: FMT.h:341
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
Base class for a planner.
Definition: Planner.h:232
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition: FMT.h:280
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: FMT.h:116
base::State * getState() const
Get the state associated with the motion.
Definition: FMT.h:250
void saveNeighborhood(Motion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: FMT.cpp:176
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A shared pointer wrapper for ompl::base::SpaceInformation.
base::State * goalState_
Goal state caching to accelerate cost to go heuristic computation.
Definition: FMT.h:493
void setState(base::State *state)
Set the state associated with the motion.
Definition: FMT.h:244
Definition of an abstract state.
Definition: State.h:49
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:268
bool getCacheCC() const
Get the state of the collision check caching.
Definition: FMT.h:181
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner. ...
Definition: FMT.h:168
Abstract definition of optimization objectives.
std::vector< Motion * > children_
The set of motions descending from the current motion.
Definition: FMT.h:344
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: FMT.h:187
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: FMT.h:122
The exception type for ompl.
Definition: Exception.h:46
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: FMT.h:490
A shared pointer wrapper for ompl::base::OptimizationObjective.
void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
For each goal region, check to see if any of the sampled states fall within that region. If not, add a goal state from that region directly into the set of vertices. In this way, FMT is able to find a solution, if one exists. If no sampled nodes are within a goal region, there would be no way for the algorithm to successfully find a path to that region.
Definition: FMT.cpp:248
double calculateUnitBallVolume(const unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:201
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
Definition: FMT.h:307
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:283
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:441
Motion * parent_
The parent motion in the exploration tree.
Definition: FMT.h:329
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: FMT.h:478
void updateNeighborhood(Motion *m, const std::vector< Motion *> nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
Definition: FMT.cpp:648
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:85
Comparator used to order motions in a binary heap.
Definition: FMT.h:348
base::State * state_
The state contained by the motion.
Definition: FMT.h:326
bool heuristics_
Flag to activate the cost to go heuristics.
Definition: FMT.h:456
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: FMT.h:194
bool alreadyCC(Motion *m)
Returns true if the connection to m has been already tested and failed because of a collision...
Definition: FMT.h:293
Motion * getParent() const
Get the parent motion of the current motion.
Definition: FMT.h:262