Loading...
Searching...
No Matches
EST.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2015, 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_GEOMETRIC_PLANNERS_EST_EST_
38#define OMPL_GEOMETRIC_PLANNERS_EST_EST_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include "ompl/datastructures/NearestNeighbors.h"
42#include "ompl/datastructures/PDF.h"
43#include <vector>
44
45namespace ompl
46{
47 namespace geometric
48 {
63
65 class EST : public base::Planner
66 {
67 public:
69 EST(const base::SpaceInformationPtr &si);
70
71 ~EST() override;
72
74
75 void clear() override;
76
84 void setGoalBias(double goalBias)
85 {
86 goalBias_ = goalBias;
87 }
88
90 double getGoalBias() const
91 {
92 return goalBias_;
93 }
94
100 void setRange(double distance)
101 {
102 maxDistance_ = distance;
103 // Make the neighborhood radius smaller than sampling range to
104 // keep probabilities relatively high for rejection sampling
106 }
107
109 double getRange() const
110 {
111 return maxDistance_;
112 }
113
114 void setup() override;
115
116 void getPlannerData(base::PlannerData &data) const override;
117
118 protected:
120 class Motion
121 {
122 public:
123 Motion() = default;
124
126 Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
127 {
128 }
129
130 ~Motion() = default;
131
134
136 Motion *parent{nullptr};
137
140 };
141
143 double distanceFunction(const Motion *a, const Motion *b) const
144 {
145 return si_->distance(a->state, b->state);
146 }
147
149 std::shared_ptr<NearestNeighbors<Motion *>> nn_;
150
152 std::vector<Motion *> motions_;
153
156
158 void freeMemory();
159
161 void addMotion(Motion *motion, const std::vector<Motion *> &neighbors);
162
164 base::ValidStateSamplerPtr sampler_;
165
168 double goalBias_{0.5};
169
171 double maxDistance_{0.};
172
175
178
181 };
182 }
183}
184
185#endif
A class that will hold data contained in the PDF.
Definition PDF.h:53
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
Definition of an abstract state.
Definition State.h:50
The definition of a motion.
Definition EST.h:121
PDF< Motion * >::Element * element
A pointer to the corresponding element in the probability distribution function.
Definition EST.h:139
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition EST.h:126
base::State * state
The state contained by the motion.
Definition EST.h:133
Motion * parent
The parent motion in the exploration tree.
Definition EST.h:136
EST(const base::SpaceInformationPtr &si)
Constructor.
Definition EST.cpp:43
double getGoalBias() const
Get the goal bias the planner is using.
Definition EST.h:90
PDF< Motion * > pdf_
The probability distribution function over states in the tree.
Definition EST.h:155
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition EST.cpp:75
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:168
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition EST.h:84
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition EST.h:149
std::vector< Motion * > motions_
The set of all states in the tree.
Definition EST.h:152
void setRange(double distance)
Set the range the planner is supposed to use.
Definition EST.h:100
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:246
void freeMemory()
Free the memory allocated by this planner.
Definition EST.cpp:88
double getRange() const
Get the range the planner is using.
Definition EST.h:109
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 addMotion(Motion *motion, const std::vector< Motion * > &neighbors)
Add a motion to the exploration tree.
Definition EST.cpp:230
RNG rng_
The random number generator.
Definition EST.h:177
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition EST.h:180
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition EST.h:164
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition EST.h:171
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:98
double nbrhoodRadius_
The radius considered for neighborhood.
Definition EST.h:174
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition EST.h:143
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()