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 <boost/unordered_map.hpp>
45 #include <vector>
46 
47 namespace ompl
48 {
49 
50  namespace control
51  {
52 
74  class EST : public base::Planner
75  {
76  public:
77 
79  EST(const SpaceInformationPtr &si);
80 
81  virtual ~EST();
82 
84 
85  virtual void clear();
86 
94  void setGoalBias(double goalBias)
95  {
96  goalBias_ = goalBias;
97  }
98 
100  double getGoalBias() const
101  {
102  return goalBias_;
103  }
104 
110  void setRange(double distance)
111  {
112  maxDistance_ = distance;
113  }
114 
116  double getRange() const
117  {
118  return maxDistance_;
119  }
120 
123  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
124  {
125  projectionEvaluator_ = projectionEvaluator;
126  }
127 
130  void setProjectionEvaluator(const std::string &name)
131  {
132  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
133  }
134 
137  {
138  return projectionEvaluator_;
139  }
140 
141  virtual void setup();
142 
143  virtual void getPlannerData(base::PlannerData &data) const;
144 
145  protected:
146 
151  class Motion
152  {
153  public:
154 
155  Motion() : state(NULL), control(NULL), steps(0), parent(NULL)
156  {
157  }
158 
160  Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
161  {
162  }
163 
164  ~Motion()
165  {
166  }
167 
170 
173 
175  unsigned int steps;
176 
179  };
180 
181  struct MotionInfo;
182 
185 
188 
190  struct MotionInfo
191  {
192  Motion* operator[](unsigned int i)
193  {
194  return motions_[i];
195  }
196  const Motion* operator[](unsigned int i) const
197  {
198  return motions_[i];
199  }
200  void push_back(Motion *m)
201  {
202  motions_.push_back(m);
203  }
204  unsigned int size() const
205  {
206  return motions_.size();
207  }
208  bool empty() const
209  {
210  return motions_.empty();
211  }
212  std::vector<Motion*> motions_;
213  CellPDF::Element *elem_;
214  };
215 
217  struct TreeData
218  {
219  TreeData() : grid(0), size(0)
220  {
221  }
222 
225 
227  unsigned int size;
228  };
229 
231  void freeMemory();
232 
234  void addMotion(Motion *motion);
235 
237  Motion* selectMotion();
238 
241 
244 
247 
250 
253 
255  double goalBias_;
256 
258  double maxDistance_;
259 
262 
264  CellPDF pdf_;
265 
268  };
269 
270  }
271 }
272 
273 #endif
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: EST.h:160
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
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:51
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition: EST.h:252
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:123
Control * control
The control contained by the motion.
Definition: EST.h:172
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
Definition of an abstract control.
Definition: Control.h:48
DirectedControlSamplerPtr controlSampler_
Directed control sampler.
Definition: EST.h:243
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: EST.cpp:222
CellPDF pdf_
The PDF used for selecting a cell from which to sample a motion.
Definition: EST.h:264
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: EST.cpp:248
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: EST.h:267
A struct containing an array of motions and a corresponding PDF element.
Definition: EST.h:190
Expansive Space Trees.
Definition: EST.h:74
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: EST.h:240
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:255
TreeData tree_
The exploration tree constructed by this algorithm.
Definition: EST.h:249
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: EST.cpp:60
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: EST.h:136
Main namespace. Contains everything in this library.
Definition: Cost.h:42
The data contained by a tree of exploration.
Definition: EST.h:217
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
unsigned int steps
The number of steps the control is applied for.
Definition: EST.h:175
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: EST.cpp:70
Base class for a planner.
Definition: Planner.h:232
A boost shared pointer wrapper for ompl::control::DirectedControlSampler.
Motion * parent
The parent motion in the exploration tree.
Definition: EST.h:178
unsigned int size
The total number of motions in the grid.
Definition: EST.h:227
A boost 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:50
Representation of a motion.
Definition: EST.h:151
A boost shared pointer wrapper for ompl::control::SpaceInformation.
Definition of a cell in this grid.
Definition: Grid.h:59
RNG rng_
The random number generator.
Definition: EST.h:261
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: EST.h:110
double getGoalBias() const
Get the goal bias the planner is using.
Definition: EST.h:100
base::State * state
The state contained by the motion.
Definition: EST.h:169
void freeMemory()
Free the memory allocated by this planner.
Definition: EST.cpp:82
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:94
PDF< GridCell * > CellPDF
A PDF of grid cells.
Definition: EST.h:187
double getRange() const
Get the range the planner is using.
Definition: EST.h:116
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: EST.h:258
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
Space information containing necessary information for planning with controls. setup() needs to be ca...
Grid< MotionInfo >::Cell GridCell
A grid cell.
Definition: EST.h:181
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: EST.h:130
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: EST.h:246
Grid< MotionInfo > grid
A grid where each cell contains an array of motions.
Definition: EST.h:224
EST(const SpaceInformationPtr &si)
Constructor.
Definition: EST.cpp:43