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() : state(nullptr), control(nullptr), steps(0), parent(nullptr)
152  {
153  }
154 
157  : state(si->allocState()), control(si->allocControl()), steps(0), parent(nullptr)
158  {
159  }
160 
161  ~Motion() = default;
162 
165 
168 
170  unsigned int steps;
171 
174  };
175 
176  struct MotionInfo;
177 
180 
183 
185  struct MotionInfo
186  {
187  Motion *operator[](unsigned int i)
188  {
189  return motions_[i];
190  }
191  const Motion *operator[](unsigned int i) const
192  {
193  return motions_[i];
194  }
195  void push_back(Motion *m)
196  {
197  motions_.push_back(m);
198  }
199  unsigned int size() const
200  {
201  return motions_.size();
202  }
203  bool empty() const
204  {
205  return motions_.empty();
206  }
207  std::vector<Motion *> motions_;
208  CellPDF::Element *elem_;
209  };
210 
212  struct TreeData
213  {
214  TreeData() : grid(0), size(0)
215  {
216  }
217 
220 
222  unsigned int size;
223  };
224 
226  void freeMemory();
227 
229  void addMotion(Motion *motion);
230 
232  Motion *selectMotion();
233 
236 
239 
242 
245 
249 
252  double goalBias_;
253 
255  double maxDistance_;
256 
259 
261  CellPDF pdf_;
262 
265  };
266  }
267 }
268 
269 #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:156
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:97
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition: EST.cpp:228
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:248
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:167
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:238
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: EST.cpp:70
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: EST.cpp:222
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:248
CellPDF pdf_
The PDF used for selecting a cell from which to sample a motion.
Definition: EST.h:261
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:264
A struct containing an array of motions and a corresponding PDF element.
Definition: EST.h:185
Expansive Space Trees.
Definition: EST.h:73
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: EST.h:235
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:252
TreeData tree_
The exploration tree constructed by this algorithm.
Definition: EST.h:244
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
The data contained by a tree of exploration.
Definition: EST.h:212
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:170
Base class for a planner.
Definition: Planner.h:232
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Motion * parent
The parent motion in the exploration tree.
Definition: EST.h:173
unsigned int size
The total number of motions in the grid.
Definition: EST.h:222
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:258
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:164
void freeMemory()
Free the memory allocated by this planner.
Definition: EST.cpp:82
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: EST.cpp:60
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:255
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
Space information containing necessary information for planning with controls. setup() needs to be ca...
Grid< MotionInfo >::Cell GridCell
A grid cell.
Definition: EST.h:176
PDF< GridCell * > CellPDF
A PDF of grid cells.
Definition: EST.h:182
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:241
Grid< MotionInfo > grid
A grid where each cell contains an array of motions.
Definition: EST.h:219
EST(const SpaceInformationPtr &si)
Constructor.
Definition: EST.cpp:43