EST.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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 the 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: Ryan Luna */
36 
37 #ifndef OMPL_CONTROL_PLANNERS_EST_EST_
38 #define OMPL_CONTROL_PLANNERS_EST_EST_
39 
40 #include "ompl/datastructures/Grid.h"
41 #include "ompl/control/planners/PlannerIncludes.h"
42 #include "ompl/base/ProjectionEvaluator.h"
43 #include "ompl/datastructures/PDF.h"
44 #include <unordered_map>
45 #include <vector>
46 
47 namespace ompl
48 {
49  namespace control
50  {
73  class EST : public base::Planner
74  {
75  public:
77  EST(const SpaceInformationPtr &si);
78 
79  ~EST() override;
80 
82 
83  void clear() override;
84 
92  void setGoalBias(double goalBias)
93  {
94  goalBias_ = goalBias;
95  }
96 
98  double getGoalBias() const
99  {
100  return goalBias_;
101  }
102 
108  void setRange(double distance)
109  {
110  maxDistance_ = distance;
111  }
112 
114  double getRange() const
115  {
116  return maxDistance_;
117  }
118 
121  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
122  {
123  projectionEvaluator_ = projectionEvaluator;
124  }
125 
128  void setProjectionEvaluator(const std::string &name)
129  {
130  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
131  }
132 
135  {
136  return projectionEvaluator_;
137  }
138 
139  void setup() override;
140 
141  void getPlannerData(base::PlannerData &data) const override;
142 
143  protected:
148  class Motion
149  {
150  public:
151  Motion() = default;
152 
155  : state(si->allocState()), control(si->allocControl())
156  {
157  }
158 
159  ~Motion() = default;
160 
162  base::State *state{nullptr};
163 
165  Control *control{nullptr};
166 
168  unsigned int steps{0};
169 
171  Motion *parent{nullptr};
172  };
173 
174  struct MotionInfo;
175 
178 
181 
183  struct MotionInfo
184  {
185  Motion *operator[](unsigned int i)
186  {
187  return motions_[i];
188  }
189  const Motion *operator[](unsigned int i) const
190  {
191  return motions_[i];
192  }
193  void push_back(Motion *m)
194  {
195  motions_.push_back(m);
196  }
197  unsigned int size() const
198  {
199  return motions_.size();
200  }
201  bool empty() const
202  {
203  return motions_.empty();
204  }
205  std::vector<Motion *> motions_;
206  CellPDF::Element *elem_;
207  };
208 
210  struct TreeData
211  {
212  TreeData() = default;
213 
216 
218  unsigned int size{0};
219  };
220 
222  void freeMemory();
223 
225  void addMotion(Motion *motion);
226 
228  Motion *selectMotion();
229 
232 
235 
238 
241 
245 
248  double goalBias_{0.05};
249 
251  double maxDistance_{0.};
252 
255 
257  CellPDF pdf_;
258 
261  };
262  }
263 }
264 
265 #endif
double getRange() const
Get the range the planner is using.
Definition: EST.h:114
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: EST.h:154
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
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: EST.cpp:94
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition: EST.cpp:225
Representation of a simple grid.
Definition: Grid.h:50
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition: EST.h:244
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state...
Definition: EST.h:121
Control * control
The control contained by the motion.
Definition: EST.h:165
double getGoalBias() const
Get the goal bias the planner is using.
Definition: EST.h:98
A shared pointer wrapper for ompl::base::ValidStateSampler.
Definition of an abstract control.
Definition: Control.h:47
DirectedControlSamplerPtr controlSampler_
Directed control sampler.
Definition: EST.h:234
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: EST.cpp:67
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: EST.cpp:219
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: EST.cpp:245
CellPDF pdf_
The PDF used for selecting a cell from which to sample a motion.
Definition: EST.h:257
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: EST.h:260
A struct containing an array of motions and a corresponding PDF element.
Definition: EST.h:183
Expansive Space Trees.
Definition: EST.h:73
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: EST.h:231
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: EST.h:134
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: EST.h:248
TreeData tree_
The exploration tree constructed by this algorithm.
Definition: EST.h:240
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
The data contained by a tree of exploration.
Definition: EST.h:210
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
unsigned int steps
The number of steps the control is applied for.
Definition: EST.h:168
Base class for a planner.
Definition: Planner.h:223
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Motion * parent
The parent motion in the exploration tree.
Definition: EST.h:171
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:49
Representation of a motion.
Definition: EST.h:148
A class that will hold data contained in the PDF.
Definition: PDF.h:52
A shared pointer wrapper for ompl::control::SpaceInformation.
Definition of a cell in this grid.
Definition: Grid.h:57
RNG rng_
The random number generator.
Definition: EST.h:254
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: EST.h:108
base::State * state
The state contained by the motion.
Definition: EST.h:162
void freeMemory()
Free the memory allocated by this planner.
Definition: EST.cpp:79
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: EST.cpp:57
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: EST.h:92
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: EST.h:251
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
Space information containing necessary information for planning with controls. setup() needs to be ca...
Grid< MotionInfo >::Cell GridCell
A grid cell.
Definition: EST.h:174
PDF< GridCell * > CellPDF
A PDF of grid cells.
Definition: EST.h:180
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: EST.h:128
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: EST.h:237
EST(const SpaceInformationPtr &si)
Constructor.
Definition: EST.cpp:43