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) and Tim Wheeler (Stanford) */
37 /* Algorithm design: Lucas Janson (Stanford) and Marco Pavone (Stanford) */
38 /* Acknowledgements for insightful comments: Edward Schmerling (Stanford),
39  * Oren Salzman (Tel Aviv University), Joseph Starek (Stanford), and Evan Clark (Stanford) */
40 
41 #ifndef OMPL_GEOMETRIC_PLANNERS_FMT_
42 #define OMPL_GEOMETRIC_PLANNERS_FMT_
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 
51 
52 namespace ompl
53 {
54 
55  namespace geometric
56  {
57 
77  class FMT : public ompl::base::Planner
78  {
79  public:
80 
81  FMT(const base::SpaceInformationPtr &si);
82 
83  virtual ~FMT();
84 
85  virtual void setup();
86 
88 
89  virtual void clear();
90 
91  virtual void getPlannerData(base::PlannerData &data) const;
92 
98  void setNumSamples(const unsigned int numSamples)
99  {
100  numSamples_ = numSamples;
101  }
102 
104  unsigned int getNumSamples() const
105  {
106  return numSamples_;
107  }
108 
119  void setRadiusMultiplier(const double radiusMultiplier)
120  {
121  if (radiusMultiplier <= 0.0)
122  throw Exception("Radius multiplier must be greater than zero");
123  radiusMultiplier_ = radiusMultiplier;
124  }
125 
128  double getRadiusMultiplier() const
129  {
130  return radiusMultiplier_;
131  }
132 
136  void setFreeSpaceVolume(const double freeSpaceVolume)
137  {
138  if (freeSpaceVolume < 0.0)
139  throw Exception("Free space volume should be greater than zero");
140  freeSpaceVolume_ = freeSpaceVolume;
141  }
142 
145  double getFreeSpaceVolume() const
146  {
147  return freeSpaceVolume_;
148  }
149 
150  protected:
153  class Motion
154  {
155  public:
156 
164  enum SetType { SET_NULL, SET_H, SET_W };
165 
166  Motion()
167  : state_(NULL), parent_(NULL), cost_(0.0), currentSet_(SET_NULL)
168  {
169  }
170 
173  : state_(si->allocState()), parent_(NULL), cost_(0.0), currentSet_(SET_NULL)
174  {
175  }
176 
177  ~Motion()
178  {
179  }
180 
182  void setState(base::State *state)
183  {
184  state_ = state;
185  }
186 
189  {
190  return state_;
191  }
192 
194  void setParent(Motion *parent)
195  {
196  parent_ = parent;
197  }
198 
200  Motion* getParent() const
201  {
202  return parent_;
203  }
204 
206  void setCost(const base::Cost cost)
207  {
208  cost_ = cost;
209  }
210 
213  {
214  return cost_;
215  }
216 
218  void setSetType(const SetType currentSet)
219  {
220  currentSet_ = currentSet;
221  }
222 
225  {
226  return currentSet_;
227  }
228 
229  protected:
230 
233 
236 
239 
242  };
243 
246  {
247  MotionCompare() : opt_(NULL)
248  {
249  }
250 
251  /* Returns true if m1 is lower cost than m2. m1 and m2 must
252  have been instantiated with the same optimization objective */
253  bool operator()(const Motion *m1, const Motion *m2) const
254  {
255  return opt_->isCostBetterThan(m1->getCost(), m2->getCost());
256  }
257 
259  };
260 
265  double distanceFunction(const Motion *a, const Motion *b) const
266  {
267  return opt_->motionCost(a->getState(), b->getState()).value();
268  }
269 
271  void freeMemory();
272 
276 
284 
286  double calculateUnitBallVolume(const unsigned int dimension) const;
287 
297  double calculateRadius(unsigned int dimension, unsigned int n) const;
298 
300  void saveNeighborhood(Motion *m, const double r);
301 
305  void traceSolutionPathThroughTree(Motion *goalMotion);
306 
313  bool expandTreeFromNode(Motion *&z, const double r);
314 
318 
323  MotionBinHeap H_;
324 
327  std::map<Motion*, MotionBinHeap::Element*> hElements_;
328 
331  std::map<Motion*, std::vector<Motion*> > neighborhoods_;
332 
334  unsigned int numSamples_;
335 
338 
352 
354  boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
355 
358 
361 
364 
365  };
366  }
367 }
368 
369 
370 #endif // OMPL_GEOMETRIC_PLANNERS_FMT_
MotionBinHeap H_
A binary heap for storing explored motions in cost-to-come sorted order. The motions in H have been e...
Definition: FMT.h:323
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
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:181
unsigned int numSamples_
The number of samples to use when planning.
Definition: FMT.h:334
SetType
The FMT* planner begins with all nodes included in set W "Waiting for optimal connection". As nodes are connected to the tree, they are transferred into set H "Horizon of explored tree." Once a node in H is no longer close enough to the frontier to connect to any more nodes in W, it is removed from H. These three SetTypes are flags indicating which set the node belongs to; H, W, or neither.
Definition: FMT.h:164
void setParent(Motion *parent)
Set the parent motion of the current motion.
Definition: FMT.h:194
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:119
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:72
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:136
SetType currentSet_
The flag indicating which set a motion belongs to.
Definition: FMT.h:241
base::StateSamplerPtr sampler_
State sampler.
Definition: FMT.h:357
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:265
A boost shared pointer wrapper for ompl::base::StateSampler.
base::Cost cost_
The cost of this motion.
Definition: FMT.h:238
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: FMT.h:360
double freeSpaceVolume_
The volume of the free configuration space.
Definition: FMT.h:337
base::Cost getCost() const
Get the cost-to-come for the current motion.
Definition: FMT.h:212
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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:331
Representation of a motion.
Definition: FMT.h:153
void freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:93
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:98
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: FMT.cpp:108
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:330
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: FMT.cpp:238
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: FMT.h:172
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: FMT.h:104
ompl::BinaryHeap< Motion *, MotionCompare > MotionBinHeap
A binary heap for storing explored motions in cost-to-come sorted order.
Definition: FMT.h:317
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone...
Definition: FMT.h:77
boost::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: FMT.h:354
Abstract definition of a goal region that can be sampled.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: FMT.h:128
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:218
Motion * getParent() const
Get the parent motion of the current motion.
Definition: FMT.h:200
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
void setState(base::State *state)
Set the state associated with the motion.
Definition: FMT.h:182
Definition of an abstract state.
Definition: State.h:50
base::State * getState() const
Get the state associated with the motion.
Definition: FMT.h:188
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:206
Abstract definition of optimization objectives.
The exception type for ompl.
Definition: Exception.h:47
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: FMT.h:363
A boost 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:204
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner. ...
Definition: FMT.h:145
Motion * parent_
The parent motion in the exploration tree.
Definition: FMT.h:235
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: FMT.cpp:121
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:173
SetType getSetType() const
Get the set that this motion belongs to.
Definition: FMT.h:224
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: FMT.h:351
bool expandTreeFromNode(Motion *&z, const double r)
Complete one iteration of the main loop of the FMT* algorithm: Find all nodes in set W within a radiu...
Definition: FMT.cpp:350
void saveNeighborhood(Motion *m, const double r)
Save the neighbors within a given radius of a state.
Definition: FMT.cpp:141
Comparator used to order motions in a binary heap.
Definition: FMT.h:245
base::State * state_
The state contained by the motion.
Definition: FMT.h:232
double calculateUnitBallVolume(const unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:163
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
std::map< Motion *, MotionBinHeap::Element * > hElements_
A map of all of the elements stored within the MotionBinHeap H, used to convert between Motion and El...
Definition: FMT.h:327