Loading...
Searching...
No Matches
ProjEST.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_EST_PROJEST_
38#define OMPL_GEOMETRIC_PLANNERS_EST_PROJEST_
39
40#include "ompl/datastructures/Grid.h"
41#include "ompl/geometric/planners/PlannerIncludes.h"
42#include "ompl/base/ProjectionEvaluator.h"
43#include "ompl/datastructures/PDF.h"
44#include <vector>
45
46namespace ompl
47{
48 namespace geometric
49 {
72 class ProjEST : public base::Planner
73 {
74 public:
76 ProjEST(const base::SpaceInformationPtr &si);
77
78 ~ProjEST() override;
79
81
82 void clear() override;
83
91 void setGoalBias(double goalBias)
92 {
93 goalBias_ = goalBias;
94 }
95
97 double getGoalBias() const
98 {
99 return goalBias_;
100 }
101
107 void setRange(double distance)
108 {
109 maxDistance_ = distance;
110 }
111
113 double getRange() const
114 {
115 return maxDistance_;
116 }
117
120 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
121 {
122 projectionEvaluator_ = projectionEvaluator;
123 }
124
127 void setProjectionEvaluator(const std::string &name)
128 {
129 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
130 }
131
133 const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
134 {
136 }
137
138 void setup() override;
139
140 void getPlannerData(base::PlannerData &data) const override;
141
142 protected:
144 class Motion
145 {
146 public:
147 Motion() = default;
148
150 Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
151 {
152 }
153
154 ~Motion() = default;
155
158
160 Motion *parent{nullptr};
161 };
162
163 struct MotionInfo;
164
167
170
173 {
174 Motion *operator[](unsigned int i)
175 {
176 return motions_[i];
177 }
178 const Motion *operator[](unsigned int i) const
179 {
180 return motions_[i];
181 }
182 void push_back(Motion *m)
183 {
184 motions_.push_back(m);
185 }
186 unsigned int size() const
187 {
188 return motions_.size();
189 }
190 bool empty() const
191 {
192 return motions_.empty();
193 }
194 std::vector<Motion *> motions_;
195 CellPDF::Element *elem_;
196 };
197
199 struct TreeData
200 {
201 TreeData() = default;
202
205
207 unsigned int size{0};
208 };
209
211 void freeMemory();
212
214 void addMotion(Motion *motion);
215
217 Motion *selectMotion();
218
220 base::ValidStateSamplerPtr sampler_;
221
224
227 base::ProjectionEvaluatorPtr projectionEvaluator_;
228
231 double goalBias_{0.05};
232
234 double maxDistance_{0.};
235
238
241
244 };
245 }
246}
247
248#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...
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
Definition of an abstract state.
Definition State.h:50
The definition of a motion.
Definition ProjEST.h:145
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition ProjEST.h:150
Motion * parent
The parent motion in the exploration tree.
Definition ProjEST.h:160
base::State * state
The state contained by the motion.
Definition ProjEST.h:157
Expansive Space Trees.
Definition ProjEST.h:73
TreeData tree_
The exploration tree constructed by this algorithm.
Definition ProjEST.h:223
RNG rng_
The random number generator.
Definition ProjEST.h:237
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition ProjEST.h:243
double getRange() const
Get the range the planner is using.
Definition ProjEST.h:113
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition ProjEST.cpp:57
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 ProjEST.cpp:91
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Definition ProjEST.cpp:197
CellPDF pdf_
The PDF used for selecting a cell from which to sample a motion.
Definition ProjEST.h:240
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition ProjEST.h:133
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 ProjEST.cpp:217
void freeMemory()
Free the memory allocated by this planner.
Definition ProjEST.cpp:78
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition ProjEST.h:227
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition ProjEST.h:231
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition ProjEST.h:127
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition ProjEST.h:220
double getGoalBias() const
Get the goal bias the planner is using.
Definition ProjEST.h:97
Motion * selectMotion()
Select a motion to continue the expansion of the tree from.
Definition ProjEST.cpp:191
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition ProjEST.h:234
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition ProjEST.h:91
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition ProjEST.cpp:67
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state.
Definition ProjEST.h:120
void setRange(double distance)
Set the range the planner is supposed to use.
Definition ProjEST.h:107
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 ProjEST.h:173
The data contained by a tree of exploration.
Definition ProjEST.h:200
unsigned int size
The total number of motions in the grid.
Definition ProjEST.h:207
Grid< MotionInfo > grid
A grid where each cell contains an array of motions.
Definition ProjEST.h:204