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() = default;
148 
150  Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
151  {
152  }
153 
154  ~Motion() = default;
155 
157  base::State *state{nullptr};
158 
160  Motion *parent{nullptr};
161  };
162 
163  struct MotionInfo;
164 
167 
170 
172  struct MotionInfo
173  {
174  Motion *operator[](unsigned int i)
175  {
176  return motions_[i];
177  }
178  const Motion *operator[](unsigned int i) const
179  {
180  return motions_[i];
181  }
182  void push_back(Motion *m)
183  {
184  motions_.push_back(m);
185  }
186  unsigned int size() const
187  {
188  return motions_.size();
189  }
190  bool empty() const
191  {
192  return motions_.empty();
193  }
194  std::vector<Motion *> motions_;
195  CellPDF::Element *elem_;
196  };
197 
199  struct TreeData
200  {
201  TreeData() = default;
202 
205 
207  unsigned int size{0};
208  };
209 
211  void freeMemory();
212 
214  void addMotion(Motion *motion);
215 
217  Motion *selectMotion();
218 
221 
224 
228 
231  double goalBias_{0.05};
232 
234  double maxDistance_{0.};
235 
238 
240  CellPDF pdf_;
241 
244  };
245  }
246 }
247 
248 #endif
The data contained by a tree of exploration.
Definition: ProjEST.h:199
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
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:237
Motion * parent
The parent motion in the exploration tree.
Definition: ProjEST.h:160
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: ProjEST.cpp:191
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:91
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:67
double getRange() const
Get the range the planner is using.
Definition: ProjEST.h:113
Grid< MotionInfo >::Cell GridCell
A grid cell.
Definition: ProjEST.h:163
base::State * state
The state contained by the motion.
Definition: ProjEST.h:157
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:227
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:197
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:223
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:217
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: ProjEST.cpp:57
PDF< GridCell * > CellPDF
A PDF of grid cells.
Definition: ProjEST.h:169
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:150
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:240
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: ProjEST.h:234
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:406
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:231
void freeMemory()
Free the memory allocated by this planner.
Definition: ProjEST.cpp:78
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: ProjEST.h:243
A struct containing an array of motions and a corresponding PDF element.
Definition: ProjEST.h:172
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: ProjEST.h:220
TreeData tree_
The exploration tree constructed by this algorithm.
Definition: ProjEST.h:223
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: ProjEST.h:127