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 
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>
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 
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 
237 
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 
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 
274 
276  std::vector<base::State *> prevSolution_;
277 
280 
283  };
284  }
285 }
286 
287 #endif
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
void freeMemory()
Free the memory allocated by this planner.
Definition: SST.cpp:114
Motion * parent_
The parent motion in the exploration tree.
Definition: SST.h:197
SST(const base::SpaceInformationPtr &si)
Constructor.
Definition: SST.cpp:45
void setSelectionRadius(double selectionRadius)
Set the radius for selecting nodes relative to random sample.
Definition: SST.h:121
base::Cost prevSolutionCost_
The best solution cost we found so far.
Definition: SST.h:279
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: SST.h:254
bool inactive_
If inactive, this node is not considered for selection.
Definition: SST.h:203
Motion * selectNode(Motion *sample)
Finds the best node in the tree withing the selection radius around a random sample.
Definition: SST.cpp:147
A shared pointer wrapper for ompl::base::StateSampler.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned numChildren_
Number of children.
Definition: SST.h:200
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state and the control.
Definition: SST.h:176
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:261
void setGoalBias(double goalBias)
Definition: SST.h:86
base::StateSamplerPtr sampler_
State sampler.
Definition: SST.h:251
void setPruningRadius(double pruningRadius)
Set the radius for pruning nodes.
Definition: SST.h:142
double getPruningRadius() const
Get the pruning radius the planner is using.
Definition: SST.h:148
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: SST.h:155
Representation of a motion.
Definition: SST.h:170
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
std::shared_ptr< NearestNeighbors< Motion * > > witnesses_
A nearest-neighbors datastructure containing the tree of witness motions.
Definition: SST.h:257
base::State * monteCarloProp(Motion *m)
Randomly propagate a new edge.
Definition: SST.cpp:200
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
std::vector< base::State * > prevSolution_
The best solution we found so far.
Definition: SST.h:276
Base class for a planner.
Definition: Planner.h:223
double getSelectionRadius() const
Get the selection radius the planner is using.
Definition: SST.h:127
double pruningRadius_
The radius for determining the size of the pruning region.
Definition: SST.h:270
double getRange() const
Get the range the planner is using.
Definition: SST.h:108
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
base::OptimizationObjectivePtr opt_
The optimization objective.
Definition: SST.h:282
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: SST.h:102
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:398
RNG rng_
The random number generator.
Definition: SST.h:273
A shared pointer wrapper for ompl::base::OptimizationObjective.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SST.cpp:63
double getGoalBias() const
Get the goal bias the planner is using.
Definition: SST.h:92
double selectionRadius_
The radius for determining the node selected for extension.
Definition: SST.h:267
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
base::State * state_
The state contained by the motion.
Definition: SST.h:194
void clear() override
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Definition: SST.cpp:102
Witness * findClosestWitness(Motion *node)
Find the closest witness node to a newly generated potential node.
Definition: SST.cpp:176
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: SST.h:245
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: SST.h:264
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continue solving for some amount of time. Return true if solution was found.
Definition: SST.cpp:216