ProjEST.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 the Willow Garage 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: Ioan Sucan */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_EST_PROJEST_
38 #define OMPL_GEOMETRIC_PLANNERS_EST_PROJEST_
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 <vector>
45 
46 namespace ompl
47 {
48  namespace geometric
49  {
72  class ProjEST : public base::Planner
73  {
74  public:
77 
78  ~ProjEST() override;
79 
81 
82  void clear() override;
83 
91  void setGoalBias(double goalBias)
92  {
93  goalBias_ = goalBias;
94  }
95 
97  double getGoalBias() const
98  {
99  return goalBias_;
100  }
101 
107  void setRange(double distance)
108  {
109  maxDistance_ = distance;
110  }
111 
113  double getRange() const
114  {
115  return maxDistance_;
116  }
117 
120  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
121  {
122  projectionEvaluator_ = projectionEvaluator;
123  }
124 
127  void setProjectionEvaluator(const std::string &name)
128  {
129  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
130  }
131 
134  {
135  return projectionEvaluator_;
136  }
137 
138  void setup() override;
139 
140  void getPlannerData(base::PlannerData &data) const override;
141 
142  protected:
144  class Motion
145  {
146  public:
147  Motion() : state(nullptr), parent(nullptr)
148  {
149  }
150 
152  Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
153  {
154  }
155 
156  ~Motion() = default;
157 
160 
163  };
164 
165  struct MotionInfo;
166 
169 
172 
174  struct MotionInfo
175  {
176  Motion *operator[](unsigned int i)
177  {
178  return motions_[i];
179  }
180  const Motion *operator[](unsigned int i) const
181  {
182  return motions_[i];
183  }
184  void push_back(Motion *m)
185  {
186  motions_.push_back(m);
187  }
188  unsigned int size() const
189  {
190  return motions_.size();
191  }
192  bool empty() const
193  {
194  return motions_.empty();
195  }
196  std::vector<Motion *> motions_;
197  CellPDF::Element *elem_;
198  };
199 
201  struct TreeData
202  {
203  TreeData() : grid(0), size(0)
204  {
205  }
206 
209 
211  unsigned int size;
212  };
213 
215  void freeMemory();
216 
218  void addMotion(Motion *motion);
219 
221  Motion *selectMotion();
222 
225 
228 
232 
235  double goalBias_;
236 
238  double maxDistance_;
239 
242 
244  CellPDF pdf_;
245 
248  };
249  }
250 }
251 
252 #endif
Grid< MotionInfo > grid
A grid where each cell contains an array of motions.
Definition: ProjEST.h:208
The data contained by a tree of exploration.
Definition: ProjEST.h:201
ProjEST(const base::SpaceInformationPtr &si)
Constructor.
Definition: ProjEST.cpp:43
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
unsigned int size
The total number of motions in the grid.
Definition: ProjEST.h:211
double getGoalBias() const
Get the goal bias the planner is using.
Definition: ProjEST.h:97
Representation of a simple grid.
Definition: Grid.h:50
A shared pointer wrapper for ompl::base::ValidStateSampler.
RNG rng_
The random number generator.
Definition: ProjEST.h:241
Motion * parent
The parent motion in the exploration tree.
Definition: ProjEST.h:162
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: ProjEST.cpp:194
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: ProjEST.cpp:94
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards, the algorithm may in fact choose the actual goal state, if it knows it, with some probability. This probability is a real number between 0.0 and 1.0; its value should usually be around 0.05 and should not be too large. It is probably a good idea to use the default value.
Definition: ProjEST.h:91
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: ProjEST.cpp:70
double getRange() const
Get the range the planner is using.
Definition: ProjEST.h:113
Grid< MotionInfo >::Cell GridCell
A grid cell.
Definition: ProjEST.h:165
base::State * state
The state contained by the motion.
Definition: ProjEST.h:159
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: ProjEST.h:107
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition: ProjEST.h:231
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition: ProjEST.cpp:200
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Expansive Space Trees.
Definition: ProjEST.h:72
Base class for a planner.
Definition: Planner.h: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: ProjEST.cpp:220
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: ProjEST.cpp:60
PDF< GridCell * > CellPDF
A PDF of grid cells.
Definition: ProjEST.h:171
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: ProjEST.h:133
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
A class that will hold data contained in the PDF.
Definition: PDF.h:52
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: ProjEST.h:152
Definition of a cell in this grid.
Definition: Grid.h:57
The definition of a motion.
Definition: ProjEST.h:144
CellPDF pdf_
The PDF used for selecting a cell from which to sample a motion.
Definition: ProjEST.h:244
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: ProjEST.h:238
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state...
Definition: ProjEST.h:120
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: ProjEST.h:235
void freeMemory()
Free the memory allocated by this planner.
Definition: ProjEST.cpp:81
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: ProjEST.h:247
A struct containing an array of motions and a corresponding PDF element.
Definition: ProjEST.h:174
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: ProjEST.h:224
TreeData tree_
The exploration tree constructed by this algorithm.
Definition: ProjEST.h:227
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: ProjEST.h:127