SST.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rutgers the State University of New Jersey, New Brunswick
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 Rutgers 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: Zakary Littlefield */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_SST_SST_
38 #define OMPL_GEOMETRIC_PLANNERS_SST_SST_
39 
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 
43 namespace ompl
44 {
45  namespace geometric
46  {
59  class SST : public base::Planner
60  {
61  public:
63  SST(const base::SpaceInformationPtr &si);
64 
65  ~SST() override;
66 
67  void setup() override;
68 
70  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
71 
72  void getPlannerData(base::PlannerData &data) const override;
73 
77  void clear() override;
78 
86  void setGoalBias(double goalBias)
87  {
88  goalBias_ = goalBias;
89  }
90 
92  double getGoalBias() const
93  {
94  return goalBias_;
95  }
96 
102  void setRange(double distance)
103  {
104  maxDistance_ = distance;
105  }
106 
108  double getRange() const
109  {
110  return maxDistance_;
111  }
112 
121  void setSelectionRadius(double selectionRadius)
122  {
123  selectionRadius_ = selectionRadius;
124  }
125 
127  double getSelectionRadius() const
128  {
129  return selectionRadius_;
130  }
131 
142  void setPruningRadius(double pruningRadius)
143  {
144  pruningRadius_ = pruningRadius;
145  }
146 
148  double getPruningRadius() const
149  {
150  return pruningRadius_;
151  }
152 
154  template <template <typename T> class NN>
155  void setNearestNeighbors()
156  {
157  if (nn_ && nn_->size() != 0)
158  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
159  clear();
160  nn_ = std::make_shared<NN<Motion *>>();
161  witnesses_ = std::make_shared<NN<Motion *>>();
162  setup();
163  }
164 
165  protected:
170  class Motion
171  {
172  public:
173  Motion() = default;
174 
176  Motion(const base::SpaceInformationPtr &si)
177  : state_(si->allocState())
178  {
179  }
180 
181  virtual ~Motion() = default;
182 
183  virtual base::State *getState() const
184  {
185  return state_;
186  }
187  virtual Motion *getParent() const
188  {
189  return parent_;
190  }
191  base::Cost accCost_{0.};
192 
194  base::State *state_{nullptr};
195 
197  Motion *parent_{nullptr};
198 
200  unsigned numChildren_{0};
201 
203  bool inactive_{false};
204  };
205 
206  class Witness : public Motion
207  {
208  public:
209  Witness() = default;
210 
211  Witness(const base::SpaceInformationPtr &si) : Motion(si)
212  {
213  }
214  base::State *getState() const override
215  {
216  return rep_->state_;
217  }
218  Motion *getParent() const override
219  {
220  return rep_->parent_;
221  }
222 
223  void linkRep(Motion *lRep)
224  {
225  rep_ = lRep;
226  }
227 
229  Motion *rep_{nullptr};
230  };
231 
233  Motion *selectNode(Motion *sample);
234 
236  Witness *findClosestWitness(Motion *node);
237 
239  base::State *monteCarloProp(Motion *m);
240 
242  void freeMemory();
243 
245  double distanceFunction(const Motion *a, const Motion *b) const
246  {
247  return si_->distance(a->state_, b->state_);
248  }
249 
251  base::StateSamplerPtr sampler_;
252 
254  std::shared_ptr<NearestNeighbors<Motion *>> nn_;
255 
257  std::shared_ptr<NearestNeighbors<Motion *>> witnesses_;
258 
261  double goalBias_{.05};
262 
264  double maxDistance_{5.};
265 
267  double selectionRadius_{5.};
268 
270  double pruningRadius_{3.};
271 
273  RNG rng_;
274 
276  std::vector<base::State *> prevSolution_;
277 
279  base::Cost prevSolutionCost_{std::numeric_limits<double>::quiet_NaN()};
280 
282  base::OptimizationObjectivePtr opt_;
283  };
284  }
285 }
286 
287 #endif
base::OptimizationObjectivePtr opt_
The optimization objective.
Definition: SST.h:378
base::State * monteCarloProp(Motion *m)
Randomly propagate a new edge.
Definition: SST.cpp:207
bool inactive_
If inactive, this node is not considered for selection.
Definition: SST.h:299
unsigned numChildren_
Number of children.
Definition: SST.h:296
double getPruningRadius() const
Get the pruning radius the planner is using.
Definition: SST.h:244
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition: SST.h:357
void setPruningRadius(double pruningRadius)
Set the radius for pruning nodes.
Definition: SST.h:238
Definition of an abstract state.
Definition: State.h:113
Motion * parent_
The parent motion in the exploration tree.
Definition: SST.h:293
std::shared_ptr< NearestNeighbors< Motion * > > witnesses_
A nearest-neighbors datastructure containing the tree of witness motions.
Definition: SST.h:353
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Definition: SST.cpp:108
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
Witness * findClosestWitness(Motion *node)
Find the closest witness node to a newly generated potential node.
Definition: SST.cpp:183
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: SST.h:360
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
Definition: SST.cpp:224
base::StateSamplerPtr sampler_
State sampler.
Definition: SST.h:347
void freeMemory()
Free the memory allocated by this planner.
Definition: SST.cpp:121
SST(const base::SpaceInformationPtr &si)
Constructor.
Definition: SST.cpp:45
double getGoalBias() const
Get the goal bias the planner is using.
Definition: SST.h:188
base::Cost prevSolutionCost_
The best solution cost we found so far.
Definition: SST.h:375
void setSelectionRadius(double selectionRadius)
Set the radius for selecting nodes relative to random sample.
Definition: SST.h:217
double selectionRadius_
The radius for determining the node selected for extension.
Definition: SST.h:363
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: SST.h:350
std::vector< base::State * > prevSolution_
The best solution we found so far.
Definition: SST.h:372
double getSelectionRadius() const
Get the selection radius the planner is using.
Definition: SST.h:223
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
base::State * state_
The state contained by the motion.
Definition: SST.h:290
Motion * selectNode(Motion *sample)
Finds the best node in the tree withing the selection radius around a random sample.
Definition: SST.cpp:154
double getRange() const
Get the range the planner is using.
Definition: SST.h:204
void setGoalBias(double goalBias)
Definition: SST.h:182
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: SST.h:341
double pruningRadius_
The radius for determining the size of the pruning region.
Definition: SST.h:366
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: SST.h:251
Motion * rep_
The node in the tree that is within the pruning radius.
Definition: SST.h:325
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: SST.h:198
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: SST.cpp:416
RNG rng_
The random number generator.
Definition: SST.h:369
Representation of a motion.
Definition: SST.h:266
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SST.cpp:65
Main namespace. Contains everything in this library.
Definition: AppBase.h:21