Loading...
Searching...
No Matches
EST.cpp
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#include "ompl/geometric/planners/est/EST.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/tools/config/SelfConfig.h"
40#include <limits>
41#include <cassert>
42
43ompl::geometric::EST::EST(const base::SpaceInformationPtr &si) : base::Planner(si, "EST")
44{
45 specs_.approximateSolutions = true;
46 specs_.directed = true;
47
48 Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange, "0.:1.:10000.");
49 Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias, "0.:.05:1.");
50}
51
52ompl::geometric::EST::~EST()
53{
54 freeMemory();
55}
56
58{
59 Planner::setup();
62
63 // Make the neighborhood radius smaller than sampling range to keep probabilities relatively high for rejection
64 // sampling
66
67 if (!nn_)
69 nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
70 {
71 return distanceFunction(a, b);
72 });
73}
74
76{
77 Planner::clear();
78 sampler_.reset();
79 freeMemory();
80 if (nn_)
81 nn_->clear();
82
83 motions_.clear();
84 pdf_.clear();
85 lastGoalMotion_ = nullptr;
86}
87
89{
90 for (auto &motion : motions_)
91 {
92 if (motion->state != nullptr)
93 si_->freeState(motion->state);
94 delete motion;
95 }
96}
97
99{
101 base::Goal *goal = pdef_->getGoal().get();
102 auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
103
104 std::vector<Motion *> neighbors;
105
106 while (const base::State *st = pis_.nextStart())
107 {
108 auto *motion = new Motion(si_);
109 si_->copyState(motion->state, st);
110
111 nn_->nearestR(motion, nbrhoodRadius_, neighbors);
112 addMotion(motion, neighbors);
113 }
114
115 if (motions_.empty())
116 {
117 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
119 }
120
121 if (!sampler_)
122 sampler_ = si_->allocValidStateSampler();
123
124 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), motions_.size());
125
126 Motion *solution = nullptr;
127 Motion *approxsol = nullptr;
128 double approxdif = std::numeric_limits<double>::infinity();
129 base::State *xstate = si_->allocState();
130 auto *xmotion = new Motion();
131
132 while (!ptc)
133 {
134 // Select a state to expand from
135 Motion *existing = pdf_.sample(rng_.uniform01());
136 assert(existing);
137
138 // Sample random state in the neighborhood (with goal biasing)
139 if ((goal_s != nullptr) && rng_.uniform01() < goalBias_ && goal_s->canSample())
140 {
141 goal_s->sampleGoal(xstate);
142
143 // Compute neighborhood of candidate motion
144 xmotion->state = xstate;
145 nn_->nearestR(xmotion, nbrhoodRadius_, neighbors);
146 }
147 else
148 {
149 // Sample a state in the neighborhood
150 if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
151 continue;
152
153 // Compute neighborhood of candidate state
154 xmotion->state = xstate;
155 nn_->nearestR(xmotion, nbrhoodRadius_, neighbors);
156
157 // reject state with probability proportional to neighborhood density
158 if (!neighbors.empty() )
159 {
160 double p = 1.0 - (1.0 / neighbors.size());
161 if (rng_.uniform01() < p)
162 continue;
163 }
164 }
165
166 // Is motion good?
167 if (si_->checkMotion(existing->state, xstate))
168 {
169 // create a motion
170 auto *motion = new Motion(si_);
171 si_->copyState(motion->state, xstate);
172 motion->parent = existing;
173
174 // add it to everything
175 addMotion(motion, neighbors);
176
177 // done?
178 double dist = 0.0;
179 bool solved = goal->isSatisfied(motion->state, &dist);
180 if (solved)
181 {
182 approxdif = dist;
183 solution = motion;
184 break;
185 }
186 if (dist < approxdif)
187 {
188 approxdif = dist;
189 approxsol = motion;
190 }
191 }
192 }
193
194 bool solved = false;
195 bool approximate = false;
196 if (solution == nullptr)
197 {
198 solution = approxsol;
199 approximate = true;
200 }
201
202 if (solution != nullptr)
203 {
204 lastGoalMotion_ = solution;
205
206 // construct the solution path
207 std::vector<Motion *> mpath;
208 while (solution != nullptr)
209 {
210 mpath.push_back(solution);
211 solution = solution->parent;
212 }
213
214 // set the solution path
215 auto path(std::make_shared<PathGeometric>(si_));
216 for (int i = mpath.size() - 1; i >= 0; --i)
217 path->append(mpath[i]->state);
218 pdef_->addSolutionPath(path, approximate, approxdif, getName());
219 solved = true;
220 }
221
222 si_->freeState(xstate);
223 delete xmotion;
224
225 OMPL_INFORM("%s: Created %u states", getName().c_str(), motions_.size());
226
227 return {solved, approximate};
228}
229
230void ompl::geometric::EST::addMotion(Motion *motion, const std::vector<Motion *> &neighbors)
231{
232 // Updating neighborhood size counts
233 for (auto neighbor : neighbors)
234 {
235 PDF<Motion *>::Element *elem = neighbor->element;
236 double w = pdf_.getWeight(elem);
237 pdf_.update(elem, w / (w + 1.));
238 }
239
240 // now add new motion to the data structures
241 motion->element = pdf_.add(motion, 1. / (neighbors.size() + 1.)); // +1 for self
242 motions_.push_back(motion);
243 nn_->add(motion);
244}
245
247{
248 Planner::getPlannerData(data);
249
250 if (lastGoalMotion_ != nullptr)
252
253 for (auto motion : motions_)
254 {
255 if (motion->parent == nullptr)
256 data.addStartVertex(base::PlannerDataVertex(motion->state));
257 else
258 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
259 }
260}
A class that will hold data contained in the PDF.
Definition PDF.h:53
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition Goal.h:63
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states.
Definition Planner.h:416
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
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
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 class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
This namespace contains sampling based planning routines shared by both planning under geometric cons...
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.