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
47namespace 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
134 const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
135 {
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
163
165 Control *control{nullptr};
166
168 unsigned int steps{0};
169
171 Motion *parent{nullptr};
172 };
173
174 struct MotionInfo;
175
178
181
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
231 base::ValidStateSamplerPtr sampler_;
232
235
238
241
244 base::ProjectionEvaluatorPtr projectionEvaluator_;
245
248 double goalBias_{0.05};
249
251 double maxDistance_{0.};
252
255
258
261 };
262 }
263}
264
265#endif
Representation of a simple grid.
Definition: Grid.h:52
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
Definition of an abstract control.
Definition: Control.h:48
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Representation of a motion.
Definition: EST.h:149
unsigned int steps
The number of steps the control is applied for.
Definition: EST.h:168
base::State * state
The state contained by the motion.
Definition: EST.h:162
Control * control
The control contained by the motion.
Definition: EST.h:165
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: EST.h:154
Motion * parent
The parent motion in the exploration tree.
Definition: EST.h:171
Expansive Space Trees.
Definition: EST.h:74
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: EST.h:237
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
void freeMemory()
Free the memory allocated by this planner.
Definition: EST.cpp:79
CellPDF pdf_
The PDF used for selecting a cell from which to sample a motion.
Definition: EST.h:257
double getGoalBias() const
Get the goal bias the planner is using.
Definition: EST.h:98
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: EST.h:251
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition: EST.cpp:225
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: EST.h:260
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: EST.h:134
double getRange() const
Get the range the planner is using.
Definition: EST.h:114
EST(const SpaceInformationPtr &si)
Constructor.
Definition: EST.cpp:43
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
DirectedControlSamplerPtr controlSampler_
Directed control sampler.
Definition: EST.h:234
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition: EST.h:244
RNG rng_
The random number generator.
Definition: EST.h:254
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: EST.h:231
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: EST.h:108
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 clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: EST.cpp:67
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
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: EST.h:128
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: EST.h:92
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition: EST.cpp:219
TreeData tree_
The exploration tree constructed by this algorithm.
Definition: EST.h:240
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
Space information containing necessary information for planning with controls. setup() needs to be ca...
Main namespace. Contains everything in this library.
Definition of a cell in this grid.
Definition: Grid.h:59
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
A struct containing an array of motions and a corresponding PDF element.
Definition: EST.h:184
The data contained by a tree of exploration.
Definition: EST.h:211
Grid< MotionInfo > grid
A grid where each cell contains an array of motions.
Definition: EST.h:215
unsigned int size
The total number of motions in the grid.
Definition: EST.h:218