Loading...
Searching...
No Matches
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 {
71
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
A container that supports probabilistic sampling over weighted data.
Definition PDF.h:49
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
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
Grid< MotionInfo >::Cell GridCell
A grid cell.
Definition EST.h:177
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
PDF< GridCell * > CellPDF
A PDF of grid cells.
Definition EST.h:180
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...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
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()
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