STRIDE.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Rice 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 Rice 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 /* Author: Bryant Gipson, Mark Moll */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_STRIDE_STRIDE_
38 #define OMPL_GEOMETRIC_PLANNERS_STRIDE_STRIDE_
39 
40 #include "ompl/datastructures/Grid.h"
41 #include "ompl/geometric/planners/PlannerIncludes.h"
42 #include "ompl/base/ProjectionEvaluator.h"
43 #include "ompl/datastructures/PDF.h"
44 #include <unordered_map>
45 #include <boost/scoped_ptr.hpp>
46 #include <vector>
47 
48 namespace ompl
49 {
50  template <typename _T>
51  class NearestNeighborsGNAT;
52 
53  namespace geometric
54  {
78  class STRIDE : public base::Planner
79  {
80  public:
82  STRIDE(const base::SpaceInformationPtr &si, bool useProjectedDistance = false, unsigned int degree = 16,
83  unsigned int minDegree = 12, unsigned int maxDegree = 18, unsigned int maxNumPtsPerLeaf = 6,
84  double estimatedDimension = 0.0);
85  ~STRIDE() override;
86 
87  void setup() override;
88 
89  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
90 
91  void clear() override;
92 
100  void setGoalBias(double goalBias)
101  {
102  goalBias_ = goalBias;
103  }
104 
106  double getGoalBias() const
107  {
108  return goalBias_;
109  }
110 
114  void setUseProjectedDistance(bool useProjectedDistance)
115  {
116  useProjectedDistance_ = useProjectedDistance;
117  }
121  bool getUseProjectedDistance() const
122  {
123  return useProjectedDistance_;
124  }
125 
127  void setDegree(unsigned int degree)
128  {
129  degree_ = degree;
130  }
132  unsigned int getDegree() const
133  {
134  return degree_;
135  }
137  void setMinDegree(unsigned int minDegree)
138  {
139  minDegree_ = minDegree;
140  }
142  unsigned int getMinDegree() const
143  {
144  return minDegree_;
145  }
147  void setMaxDegree(unsigned int maxDegree)
148  {
149  maxDegree_ = maxDegree;
150  }
152  unsigned int getMaxDegree() const
153  {
154  return maxDegree_;
155  }
158  void setMaxNumPtsPerLeaf(unsigned int maxNumPtsPerLeaf)
159  {
160  maxNumPtsPerLeaf_ = maxNumPtsPerLeaf;
161  }
164  unsigned int getMaxNumPtsPerLeaf() const
165  {
166  return maxNumPtsPerLeaf_;
167  }
171  void setEstimatedDimension(double estimatedDimension)
172  {
173  estimatedDimension_ = estimatedDimension;
174  }
178  double getEstimatedDimension() const
179  {
180  return estimatedDimension_;
181  }
182 
188  void setRange(double distance)
189  {
190  maxDistance_ = distance;
191  }
192 
194  double getRange() const
195  {
196  return maxDistance_;
197  }
204  void setMinValidPathFraction(double fraction)
205  {
206  minValidPathFraction_ = fraction;
207  }
208 
210  double getMinValidPathFraction() const
211  {
212  return minValidPathFraction_;
213  }
216  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
217  {
218  projectionEvaluator_ = projectionEvaluator;
219  }
220 
223  void setProjectionEvaluator(const std::string &name)
224  {
225  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
226  }
227 
229  const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
230  {
231  return projectionEvaluator_;
232  }
233 
234  void getPlannerData(base::PlannerData &data) const override;
235 
236  protected:
238  class Motion
239  {
240  public:
241  Motion() = default;
242 
244  Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
245  {
246  }
247 
248  ~Motion() = default;
249 
251  base::State *state{nullptr};
252 
254  Motion *parent{nullptr};
255  };
256 
258  void freeMemory();
259 
261  void setupTree();
262 
264  double distanceFunction(const Motion *a, const Motion *b) const
265  {
266  return si_->distance(a->state, b->state);
267  }
268 
270  double projectedDistanceFunction(const Motion *a, const Motion *b) const
271  {
272  unsigned int num_dims = projectionEvaluator_->getDimension();
273  Eigen::VectorXd aproj(num_dims), bproj(num_dims);
274  projectionEvaluator_->project(a->state, aproj);
275  projectionEvaluator_->project(b->state, bproj);
276  return (aproj - bproj).norm();
277  }
278 
280  void addMotion(Motion *motion);
281 
284 
286  base::ValidStateSamplerPtr sampler_;
287 
289  base::ProjectionEvaluatorPtr projectionEvaluator_;
290 
292  boost::scoped_ptr<NearestNeighborsGNAT<Motion *>> tree_;
293 
296  double goalBias_{.05};
297 
299  double maxDistance_{0.};
300 
305  unsigned int degree_;
307  unsigned int minDegree_;
309  unsigned int maxDegree_;
311  unsigned int maxNumPtsPerLeaf_;
313  double estimatedDimension_;
319  double minValidPathFraction_{.2};
320 
322  RNG rng_;
323  };
324  } // namespace geometric
325 } // namespace ompl
326 
327 #endif
void setMaxDegree(unsigned int maxDegree)
Set maximum degree of a node in the GNAT.
Definition: STRIDE.h:179
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: STRIDE.h:132
void setDegree(unsigned int degree)
Set desired degree of a node in the GNAT.
Definition: STRIDE.h:159
bool useProjectedDistance_
Whether to use distance in the projection (instead of distance in the state space) for the GNAT.
Definition: STRIDE.h:335
double projectedDistanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between projections of contained states)
Definition: STRIDE.h:302
unsigned int maxDegree_
Maximum degree of an internal node in the GNAT.
Definition: STRIDE.h:341
void setMinValidPathFraction(double fraction)
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition: STRIDE.h:236
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm can optionally use a projection to guide the exploration.
Definition: STRIDE.h:321
Definition of an abstract state.
Definition: State.h:113
base::State * state
The state contained by the motion.
Definition: STRIDE.h:283
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition: STRIDE.h:328
unsigned int minDegree_
Minimum degree of an internal node in the GNAT.
Definition: STRIDE.h:339
unsigned int getMaxDegree() const
Set maximum degree of a node in the GNAT.
Definition: STRIDE.h:184
double getGoalBias() const
Get the goal bias the planner is using.
Definition: STRIDE.h:138
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: STRIDE.h:296
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: STRIDE.cpp:82
void setUseProjectedDistance(bool useProjectedDistance)
Set whether nearest neighbors are computed based on distances in a projection of the state rather dis...
Definition: STRIDE.h:146
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: STRIDE.h:331
void setEstimatedDimension(double estimatedDimension)
Set estimated dimension of the free space, which is needed to compute the sampling weight for a node ...
Definition: STRIDE.h:203
unsigned int getDegree() const
Get desired degree of a node in the GNAT.
Definition: STRIDE.h:164
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: STRIDE.cpp:131
double getRange() const
Get the range the planner is using.
Definition: STRIDE.h:226
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
unsigned int degree_
Desired degree of an internal node in the GNAT.
Definition: STRIDE.h:337
STRIDE(const base::SpaceInformationPtr &si, bool useProjectedDistance=false, unsigned int degree=16, unsigned int minDegree=12, unsigned int maxDegree=18, unsigned int maxNumPtsPerLeaf=6, double estimatedDimension=0.0)
Constructor.
Definition: STRIDE.cpp:46
void freeMemory()
Free the memory allocated by this planner.
Definition: STRIDE.cpp:115
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: STRIDE.h:220
double minValidPathFraction_
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition: STRIDE.h:351
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition: STRIDE.cpp:232
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: STRIDE.cpp:242
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: STRIDE.h:318
The definition of a motion.
Definition: STRIDE.h:270
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: STRIDE.h:242
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: STRIDE.h:261
unsigned int getMaxNumPtsPerLeaf() const
Get maximum number of elements stored in a leaf node of the GNAT.
Definition: STRIDE.h:196
void setMinDegree(unsigned int minDegree)
Set minimum degree of a node in the GNAT.
Definition: STRIDE.h:169
double estimatedDimension_
Estimate of the local dimensionality of the free space around a state.
Definition: STRIDE.h:345
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: STRIDE.cpp:237
double getEstimatedDimension() const
Get estimated dimension of the free space, which is needed to compute the sampling weight for a node ...
Definition: STRIDE.h:210
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
RNG rng_
The random number generator.
Definition: STRIDE.h:354
void setupTree()
Initialize GNAT data structure.
Definition: STRIDE.cpp:91
Motion * parent
The parent motion in the exploration tree.
Definition: STRIDE.h:286
void setMaxNumPtsPerLeaf(unsigned int maxNumPtsPerLeaf)
Set maximum number of elements stored in a leaf node of the GNAT.
Definition: STRIDE.h:190
bool getUseProjectedDistance() const
Return whether nearest neighbors are computed based on distances in a projection of the state rather ...
Definition: STRIDE.h:153
boost::scoped_ptr< NearestNeighborsGNAT< Motion * > > tree_
The exploration tree constructed by this algorithm.
Definition: STRIDE.h:324
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: STRIDE.cpp:107
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state.
Definition: STRIDE.h:248
Main namespace. Contains everything in this library.
unsigned int getMinDegree() const
Get minimum degree of a node in the GNAT.
Definition: STRIDE.h:174
unsigned int maxNumPtsPerLeaf_
Maximum number of points stored in a leaf node in the GNAT.
Definition: STRIDE.h:343